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Abstract  of  Dissertation  Presented  to  the  Graduate  School 
of  the  University  of  Florida  in  Partial  Fulfillment  of  the 
Requirements  for  the  Degree  of  Doctor  of  Philosophy 

RAPID  GENERATION  AND  COMPUTER  GRAPHICS  ANIMATION  OF 
COLLISION-FREE  PATHS  FOR  ROBOT  MANIPULATORS 

BY 

CHING-SHIOW  TSENG 
DECEMBER  1987 

Chairman:  Dr.  Joseph  Duffy 

Major  Department:  Mechanical  Engineering 

It  is  expected  that  in  the  near  future  robot 

manipulators  will  have  the  autonomous  ability  to  avoid 
collisions  with  surrounding  objects.  The  reason  for  this 
expectation  is  that  robots  will  one  day  be  able  to  operate 
effectively  in  an  obstacle  strewn  environment  such  as  a 
house,  an  office,  a factory,  etc.  In  this  dissertation,  an 
efficient  trajectory  planning  system  is  developed  which 
rapidly  provides  a manipulator  with  a collision-free  path. 

It  is  assumed  that  the  surrounding  polyhedral  objects 
have  vertical  sides  and  flat  top  surfaces,  and  their 
positions  and  dimensions  are  known.  The  system  which  has 
been  developed  expands  the  size  of  the  objects  so  that  the 
links  of  the  manipulator  may  be  treated  as  straight  line 
segments  with  no  thickness.  An  Archimedes's  spiral  is  used 
to  determine  a col 1 is  ion- free  path  as  opposed  to  a straight 
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line  path  which  may  pass  through  the  void  of  the  wrist's 
workspace.  The  objects  outside  the  swept  volume  of  the 
manipulator  during  the  motion  from  the  initial  position  to 
the  goal  position  can  be  excluded  from  consideration. 

Each  vertex  of  the  top  surfaces  of  the  remaining 
objects  (or  obstacles)  and  the  vertical  Z-axis  of  the  fixed 
coordinate  system  determine  a vertical  plane.  These  planes 
generate  cross  sections  of  the  obstacles  and  wrist's 
workspace.  In  each  cross  section  of  the  wrist's  workspace, 
the  lower  bound  of  the  reachable  area  for  the  wrist  is 
determined  and  a reachable  wrist  point  can  be  found.  These 
reachable  wrist  points  are  used  to  determine  the  reachable 
end  effector  points.  Finally,  the  path  which  seguentially 
connects  the  initial  position,  the  reachable  end  effector 
points,  and  the  goal  position  is  modified  in  order  to 
produce  a smooth  and  efficient  collision-free  path. 

The  algorithm  developed  here  is  applied  to  the 

. . 3 

Cincinnati  Milacron  T -776  robot  in  a tightly  cluttered 
environfhent  and  is  demonstrated  via  computer  graphics 
animation.  It  is  important  to  recognize  that  this  algorithm 
can  be  applied  incorporating  minor  modifications  to  the 
polar  type,  cylindrical  type  and  Cartesian  type  robots. 


V 


CHAPTER  I 


INTRODUCTION 

Interest  in  robotics  has  grown  explosively  in  recent 
years.  As  robots  have  begun  to  proliferate  in  industry,  so 
have  the  demands  on  the  level  of  sophistication  of  their 
performance.  These  demands  include  greater  ease  of  use  and 
broader  reguirements  on  abilities.  However,  several 
technical  problem  areas  such  as  mobility,  sensing  and 
control  reguire  more  improvement  and  fundamental  research. 

Currently,  a user  need  input  the  data  of  detail  steps 
into  the  controller  to  operate  a robot.  If  a robot  has  the 
ability  to  operate  autonomously  without  colliding  with  the 
surrounding  objects,  then  it  may  accept  simple  instructions 
such  as  "move  object  A from  position  P to  Q."  This  leads  to 
the  interesting  research  of  developing  an  algorithm  to 
detect  collisions  between  a manipulator  and  objects  in  the 
workspace  and  plan  a collision-free  path  for  the 
manipulator . 

Since  Udupa  [1]  first  presented  the  problem  of 
collision  detection  and  avoidance  of  manipulators,  numerous 
research  has  been  performed  to  develop  effective  algorithms 
to  perform  motion  planning.  The  current  algorithmic 
interest  in  motion  planning  is  usually  traced  to  the  work  of 
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Lozano-Perez  [2] . The  idea  was  to  hypothesize  a simple  path 
from  an  initial  position  to  the  target  position,  test  the 
path  for  potential  collisions,  and  then  propose  a new  path. 
This  procedure  was  repeated  until  no  collisions  are  detected 
along  the  proposed  path.  Clearly,  it  is  time-consuming  to 
generate  paths  and  detect  collisions  in  this  way.  More 
recent  and  continuing  work  along  such  lines  was  reported  by 


Lozano-Perez  [3-4]  and 

Brooks  [5-6].  TheS' 

e algorithm 

were 

extended 

to  study 

several  connected 

objects  i.e. 

a 

kinematic 

chain.  They 

were  subsequently 

applied  to 

the 

obstacle  avoidance  problem  of  Cartesian  type  manipulators, 
and  to  the  determination  of  a collision-free  path  for  a 
Unimation  560  (known  as  PUMA)  manipulator  performing 
four-degree-of-f reedom  pick  and  place  operations.  In  recent 
years,  artificial  intelligence  and  optimization  techniques 
were  also  employed  to  complement  the  search  of 
collision-free  paths.  The  work  of  Myers  [7-8]  , Kuntze  and 
Schill  [9],  Faverjon  [10],  Gouzenes  [11-12],  Wang  and  Fu 
[13]  and  Dupont  and  Derby  [14]  applied  heuristic  searching 
methods  to  plan  collision-free  paths.  Such  search 
techniques  tend  to  be  time  consuming.  Most  researchers 
divided  the  free  space  into  numerous  grids  or  cubes  of 
distinct  size  and  generated  a tree  structure  to  search  for 
an  optimal  path. 

Hollerbach  and  Suh  [15]  and  Maciejewski  and  Klein  [16] 
presented  methods  of  minimizing  joint  torques  and  maximizing 
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the  distance  to  obstacles  respectively.  Further  work  on 
minimizing  motion  time  or  joint  displacement  increments  is 
reported  by  Dubowsky  and  his  associates  [17-19]  , Fenton  et 
al  [20],  Seeger  and  Paul  [21],  Luh  and  Lin  [22],  Rajan  [23] 
and  Sahar  and  Hollerbach  [24] . 

Recently,  novel  interesting  algorithms  have  been 
developed  by  Lumelsky  for  the  path  planning  of 
two-degree-of-f reedom  planar  manipulators  [25-28]  and 
Cartesian  type  manipulators  [29] . Young  and  Duffy  [30-31] 
introduced  a theory  of  the  articulation  of  planar  robots  to 
plan  motion  for  interference  avoidance.  This  theory  will  be 
extended  and  applied  to  spatial  robot  manipulators.  These 
algorithms  are  based  on  the  geometry  of  manipulators  as 
opposed  to  applying  heuristic  methods. 

In  recent  years,  computer  graphics  has  been  applied  to 
many  fields  to  reduce  development  time  and  cost,  and  help  to 
visualize  various  design.  Even  complicated  wire  frame  and 
solid  modeling  pictures  can  be  generated  in  a micro 
computer.  This  can  be  seen  from  the  work  of  Derby  [32] 
which  runs  on  an  IBM  PC/AT  and  simulates  robotic  workcells. 
However,  a workstation  specially  designed  for  generating 
graphics  is  more  efficient  and  can  display  more  realistic 
pictures . 

Obviously,  the  characteristics  of  computer  graphics  are 
very  useful  and  effective  in  studying  off-line  robot 
programming  such  as  interactive  animated  display  of  robot 
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motion  (Crane  and  Duffy  [33-34]),  robot  kinematics  and 
dynamics  analysis,  and  modeling  swept  volumes  for  collision 
detection  (Leu  [35],  Anderson  [36]  and  Mahajar  [37]). 

In  this  dissertation,  geometric  methods  are  used  to 
develop  an  algorithm  to  determine  collision-free  paths  for 
robot  manipulators  operating  in  an  obstacle-strewn 
environment  (see  figure  1.1).  After  specification  of  the 
initial  and  goal  positions  and  corresponding  orientations  of 
the  end  effector,  a collision-free  path  that  safely 
maneuvers  the  manipulator  to  the  goal  position  will  be 
generated . 

It  is  assumed  for  the  analysis  that  the  surrounding 
polyhedral  objects  are  composed  of  vertical  sides  and  flat 
top  surfaces,  and  their  locations,  sizes  and  shapes  are 
known.  The  algorithm  first  computes  an  Archimedes's  spiral, 
determined  by  the  initial  and  goal  positions  of  the  wrist, 
as  opposed  to  a straight  line  path  which  may  pass  through 
the  void  of  the  workspace.  The  polyhedral  objects  are  then 
expanded  so  that  the  links  of  the  manipulator  may  be  treated 
as  straight  line  segments.  The  position  of  each  of  the 
expanded  objects  is  checked  against  the  fan-shaped  area 
determined  by  the  origin  of  the  fixed  coordinate  system  and 
the  projection  of  the  Archimedes's  spiral  on  the  X-Y  plane. 
Only  those  objects  which  lie  inside  or  intersect  with  this 
fan-shaped  area  are  considered  to  be  obstacles.  The  other 
obiects  can  be  excluded  from  further  consideration. 
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Figure  1.1  The  robot  and  surrounding  objects 
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Each  vertex  of  the  top  surfaces  of  the  obstacles  and 
the  vertical  Z-axis  (parallel  to  the  axis  of  joint  1)  of  the 
fixed  coordinate  system  determine  a vertical  plane  (see 
figure  1.2).  This  plane  cuts  through  some  obstacles  and  the 
reachable  space  of  the  wrist  and  thus  generates  cross 
sections  of  the  obstacles  and  wrist's  reachable  space. 

The  cross  sections  of  the  reachable  space  and  obstacles 
are  mapped  through  rotational  transformations  onto  the  X-Z 
plane.  On  each  of  these  transformed  cross  sections,  the 
lowest  valid  position  for  the  second  link  (or  the  lowest 
position  for  joint  3)  can  be  determined.  Using  this 
position,  the  lower  bound  of  the  wrist's  reachable  area  can 
be  found. 

The  recommended  reachable  point  is  defined  as  the 
intersection  of  the  Archimedes's  spiral  and  the  cross 
section  of  the  wrist's  workspace.  This  recommended  point  is 
used  to  find  the  real  reachable  wrist  point  which  is  then 
inversely  transformed  to  obtain  its  true  coordinates.  These 
reachable  wrist  points  are  used  to  determine  the  reachable 
end  effector  points  by  considering  the  length  and 
orientation  of  the  end  effector.  Finally,  the  path  which 
sequentially  connects  the  initial  position,  the  reachable 
end  effector  points  and  the  goal  position  is  modified  to 
produce  a smooth  collision-free  path. 

The  algorithm  developed  is  applied  here  in  detail  to 
the  Cincinnati  Milacron  T^-776  robot  in  an  obstacle-strewn 
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Figure  1.2 


The  T -776  robot 
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environment  (see  figure  1.1).  The  response  time  for  the 
algorithm  to  generate  a collision-free  path  is  less  than  two 
seconds  on  a VAX-750  computer.  This  algorithm  can  also  be 
applied  to  the  polar  type,  the  cylindrical  type  and  the 
Cartesian  type  robots. 

In  Chapter  II,  the  method  for  expanding  obiects  is 
described  and  the  eguation  of  the  Archimedes's  spiral  is 
derived.  Each  of  the  expanded  objects  is  identified  to  be 
an  obstacle  or  a non-obstacle  which  is  discussed  in  detail. 
The  limited  angular  ranges  of  joints  2 and  3 reguired  for 
the  shoulder  and  arm  to  avoid  collision  with  the  obstacles 
are  also  determined. 

Chapter  III  describes  the  procedures  of  finding  the 
reachable  end  effector  points.  First,  the  sequence  of  the 
cross  sections  of  the  wrist's  workspace  are  sorted  by  using 
the  quicksort  algorithm.  Then,  the  cross  sections  of  the 
obstacles  are  determined  and  sorted.  The  problem  of  finding 
the  lower  bound  of  the  reachable  area  for  the  wrist  and  the 
reachable  wrist  point  is  separated  into  several  cases  which 
depend  on  the  positions  and  sizes  of  the  cross  sections  of 
the  obstacles.  Each  of  these  cases  is  discussed  separately. 

The  reachable  end  effector  points  are  determined  based 
on  the  reachable  wrist  points  and  the  orientation  of  the  end 
effector.  A detailed  discussion  of  this  determination  is 
presented.  The  safety  of  the  shoulder  and  arm  during  the 
rectilinear  motion  of  the  manipulator  between  two  sequential 
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reachable  points  is  also  proved.  Finally,  a simple  method  to 
modify  the  path  sequentially  connecting  all  reachable  points 
is  described. 

The  algorithm  developed  in  Chapters  II  and  III  is 
applied  to  the  articulated  type  robots.  It  can  be  easily 
extended  to  other  types  of  robots,  too.  In  Chapter  IV,  the 
application  of  the  algorithm  to  the  polar  type,  the 
cylindrical  type  and  the  Cartesian  type  robots  is  presented. 
The  vast  majority  of  industrial  robots  belong  to  these  four 
types  and  hence  the  algorithm  has  a wide  application. 

In  Chapter  V,  a general  description  of  computer 
graphics  animation  techniques  is  presented  as  it  applies  to 

3 

the  T -776  robot.  The  continuous  motion  of  the  robot 
operating  in  a tightly  cluttered  environment  is  demonstrated 
step  by  step  by  pictures  obtained  from  a SILICON  GRAPHICS 
IRIS  2400  workstation. 

Finally,  conclusions  and  recommendations  for  future 
study  are  discussed  in  Chapter  VI. 


CHAPTER  II 


EXPANSION  AND  IDENTIFICATION  OF  THE  OBSTACLES 

As  previously  stated,  the  dimensions  of  the  polyhedral 
objects  and  their  positions  relative  to  the  fixed  coordinate 
system  of  the  robot  are  assumed  to  be  known.  Since  the 
calculations  required  to  identify  collisions  between  a solid 
link  and  objects  in  the  workspace  are  so  complex,  it  is 
advantageous  to  consider  the  links  initially  as  line 
segments  or  vectors  of  no  volume.  In  order  to  accomplish 
this,  a common  technique  will  be  used  in  which  all  objects 
are  expanded  to  compensate  for  the  thickness  and  width  of 
the  links.  This  differs  in  some  respects  from  the  method 
due  to  Perez  and  in  this  chapter  details  are  given. 

Following  this,  an  algorithm  is  developed  to  identify 
the  obstacles  which  can  cause  interference  for  a move  from 
an  initial  to  a target  positions.  All  other  objects  can 
then  be  disregarded  from  the  analysis.  In  order  to  develop 
this  algorithm,  it  is  necessary  to  plan  the  move.  There  a 
series  of  Archimedes's  spirals  is  used  to  move  between  a 
succession  of  reachable  points  beginning  with  the  initial 
point  and  concluding  with  the  target  point. 
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2 . 1 Expansion  of  Objects 

In  figure  2.1,  the  Cincinnati  Milacron  T^-776  robot 
is  shown  together  with  a cross  section  of  an  expanded 
object.  The  cross  sections  of  the  shoulder  (link  2)  and  arm 
(link  3)  are  shown  in  figures  2.1.1  and  2.1.2  respectively. 
Before  discussing  how  to  expand  an  object,  additional 
definitions  must  be  made. 

1.  Point  P is  the  intersection  of  cross  section  A-A  of 
the  shoulder  and  shoulder  axis  which  connects  the 
centers  of  joints  2 and  3 (see  figures  2.1  and 
2.1.1). 

2.  M is  the  distance  from  point  P to  each  of  the 
bottom  corners  of  cross  section  A-A  (see  figure 
2.1.1)  . 

3.  Point  Q is  the  intersection  of  cross  section  R-B  of 
the  arm  and  arm  axis  which  connects  the  centers  of 
joint  3 and  the  wrist  (see  figures  2.1  and  2.1.2). 

4.  N is  the  distance  from  point  0 to  each  of  the 
bottom  corners  of  cross  section  B-B  (see  figure 
2.1.2)  . 

5.  V is  the  distance  from  point  0 to  the  bottom  side 
of  the  arm  (see  figure  2.1.2). 

In  figure  2.2,  the  sides  of  an  arbitrary  object  01  (02) 
are  expanded  by  the  distance  M (M).  Rectangle  D (E)  has  the 
same  size  as  the  cross  section  A-A  (B-B)  and  point  r (s) 
corresponds  to  point  P (Q).  When  point  r (s)  is  on  one  of 
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Figure  2.1 
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The  T -776  robot  with  a cross 
section  of  an  obstacle 


A-A  cross  section 


B-B  cross  section 


Figure  2.1.1 


Figure  2.1.2 
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Figure  2.2  Rectangle  D (E)  cannot 

touch  the  obstacle  01  (02) 
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the  edges  of  or  outside  the  expanded  object  01  (02),  the 
rectangle  D (E)  will  not  collide  with  object  01  (02). 
Broadly  speaking,  it  follows  that 

any  portion  of  the  sides  of  an  object  that  the 
shoulder  can  reach  is  expanded  by  the  distance  M; 
otherwise,  it  is  expanded  by  the  distance  N. 
Specifically,  six  distinct  types  of  interference 
between  the  shoulder  and  an  object  must  be  considered.  It 
is  sufficient  to  consider  interference  with  a polyhedron 
with  four  sides  and  to  examine  the  interference  of  the 
shoulder  with  pairs  of  adjacent  sides  in  turn.  This  yields 
four  distinct  cases  and  the  fifth  and  sixth  cases  occur  when 
there  is  interference  with  a single  side  only.  The  first 
four  cases  are  illustrated  by  figures  2. 3-2. 6 which  show  the 
top  of  the  four-sided  polyhedron.  The  first  vertical  axis 
(the  axis  of  joint  1)  of  the  robot  is  out  of  the  paper  and 
passes  through  point  O.  This  is  taken  as  the  positive  Z 
direction.  The  circular  arc  C with  center  0 is  the 

intersection  of  the  plane  through  the  top  of  the  polyhedron 
with  the  reachable  surface  (a  spherical  shell)  generated  by 
the  center  of  joint  3. 

In  order  to  facilitate  the  development  of  an  algorithm, 
three  of  the  four  vertices  are  tabled  a,  b and  c and  they 
are  indexed  seguentially  in  a counterclockwise  sense.  The 
details  of  the  determination  of  new  corners  for  an  object 


are  now  described. 
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Case  1;  Point  a is  outside  arc  C,  and  point  b is 
within  arc  C (see  figures  2.3  and  2.4). 

There  are  two  subcases  which  are  considered  separately. 

1.  When  Ob  X ba  < 0 (parallel  to  the  negative 

Z-direction) , the  shoulder  will  touch  point  b first  and 
hence  cannot  touch  any  other  points  on  the  line  segment  ab 
(see  figure  2.3).  Both  the  sides  ab  and  be  are  shifted  out 
a distance  M to  de  and  ef.  The  meet  of  the  lines  de  and  ef 
e is  joined  to  the  vertex  b and  the  point  i is  determined  by 
choosing  bi  = M.  The  intersections  g and  h of  the  line 
segments  de  and  ef  with  the  line  through  i and  perpendicular 
to  bi  are  then  determined.  The  corners  of  the  expanded 
object  corresponding  to  the  corner  b are  now  defined  by  the 
points  g and  h. 

2.  When  Ob  x ba  0 , the  shoulder  can  touch  all  points 
on  the  line  segment  ab  (see  figure  2.4).  The  procedure  for 
expanding  the  object  is  the  same  as  above  except  that  in 
addition  the  intersection  k of  arc  C with  the  side  ab  is 
determined.  Following  this,  the  intersection  j of  the  line 
through  k and  tangent  to  arc  C with  the  line  de  is  also 
determined  and  the  corners  of  the  expanded  object 
corresponding  to  the  corner  b are  defined  seguentially  by 
the  points  j,  g and  h. 

The  determination  of  the  coordinates  of  points  g and  h 
are  now  discussed  in  detail. 
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Figure  2.3  Ob  x ba  < 0 


a 


Figure  2.4  Ob  x ba  > 0 
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Assume  that  the  line  equations  of  lines  ab  and  be  are 
pX  + qY  = r 

uX  + vY  = w (2.1) 

where  the  constants  p,  q,  r,  u,  v and  w are  determined 
by  the  coordinates  of  points  a,  b and  c. 

When  the  directions  of  the  cross  products  Oa  x ab  and 
Ob  X be  point  to  the  positive  Z-axis,  the  equations  of  the 
lines  de  and  ef  are  given  by 

pX  + qY  = r + M / (p^  + 

uX  + vY  = w + M / (u^  + (2.2) 

Otherwise,  the  equations  of  lines  de  and  ef  are 
pX  + qY  = r - M / (p^  + q^)^^^ 

uX  + vY  = w - M / (u^  + v^)^/^  (2.3) 


where  M has  been  defined  above. 

Solving  either  of  the  pairs  of  the  simultaneous 
equations,  the  coordinates  of  the  intersection  e are 
determined  and  the  line  be  can  be  expressed  in  the  form 


IX  + mY  = n 


(2.4) 


The  coordinates  of  point  i are  calculated  and  given  by 


Xj  = Xb  + (X^  - Xb)R 


= \ + (Xe  - 


Y. 

1 


(2.5) 
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where  R = M / ((X  - x,  ) ^ + (Y  - Y,  and 

e D e D 

(Xj^,Yj^)  and  (X^,  Y^)  are  respectively  the 
coordinates  of  points  b and  e. 

The  equation  of  line  L,  which  passes  through  point  i 
and  is  perpendicular  to  line  be,  can  be  expressed  in  the 
form 

rax  - lY  = mX.  - lY.  (2.6) 

The  coordinates  of  point  g (h)  are  found  by  solving 
equations  (2.6)  and  (2.2)  ((2.6)  and  (2.3)). 

Case  2:  Point  a is  within  arc  C,  and  point  b is 

outside  arc  C (see  figures  2.5  and  2.6). 

Here  the  shoulder  cannot  reach  point  b.  The  sides  ab 
and  be  are  shifted  out  a distance  N to  line  segments  de  and 
ef.  Then,  the  procedures  to  determine  the  new  corners  g and 
h are  the  same  as  in  Case  1.  When  Ob  x ba  < 0 (see  figure 
2.5),  the  corners  of  the  expanded  object  are  thus  defined  by 
points  g and  h.  However,  when  Ob  x ba  > 0 (see  figure  2.6), 
the  shoulder  will  reach  side  ab  and  an  extra  corner  j must 
be  determined.  This  new  corner  is  determined  by  the 

intersection  of  line  de  with  the  line  which  is  tangent  to 
arc  C and  passes  through  the  intersection  k of  line  ab  and 
arc  C.  The  corners  of  the  expanded  object  are  defined  by  a 
sequence  of  points  j,  g and  h. 

Case  3:  Points  a and  b are  both  within  arc  C.  (see 

figure  2.7) 
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f 


Figure  2.5  Ob  x ba  < 0 


Figure  2.6  Ob  x ba  > 0 
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Figure  2.7  Points  a and  b are  both 
within  the  arc  C 


Figure  2.8  Side  ab  does  not  intersect 
with  the  arc  C 
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In  this  case,  sides  ab  and  be  are  shifted  out  a 
distance  M to  line  segments  de  and  ef.  The  new  corner 
points  g and  h are  determined  as  in  Case  1.  The  corners  of 
the  expanded  object  are  defined  by  the  points  g and  h. 

Case  4:  Points  a and  b are  both  outside  arc  C and  side 

ab  does  not  intersect  with  arc  C (see  figure 

2.8)  . 

It  is  necessary  to  check  at  the  outset  to  determine 

whether  side  ab  intersects  with  arc  C.  In  figure  2.8,  point 
p is  the  intersection  of  line  ab  and  the  normal  to  the  line 
which  passes  through  point  0.  When  point  p is  not  located 
between  points  a and  b,  or  when  the  distance  Op  is  greater 
than  the  radius  of  arc  C,  the  side  ab  will  not  intersect 
with  arc  C. 

In  this  case,  the  shoulder  will  not  touch  side  be. 

Both  the  sides  ab  and  be  are  shifted  out  a distance  N to  the 
line  segments  de  and  ef.  New  corners  g and  h are  then  found 
as  in  Case  1.  The  corners  of  the  expanded  object  are  now 
defined  by  the  points  g and  h. 

Case  5;  Points  a and  b are  both  outside  arc  C.  Side 
ab  intersects  arc  C.  Points  0 and  c lie  on 
opposite  sides  of  line  segment  ab  (see  figure 
2.9)  . 

In  this  case,  the  shoulder  can  touch  part  of  the  side 

ab  but  excluding  point  b.  Both  the  sides  ab  and  be  are 

shifted  out  a distance  N to  the  line  segments  de  and  ef. 
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Figure  2.9  Points  0 and  c lie  on  opposite 
sides  of  line  segment  ab 


Figure  2.10  Points  0 and  c are  on  the  same 
side  of  line  segment  ab 
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Corners  g and  h are  then  determined  as  in  Case  1.  In  figure 
2.9,  points  k and  r indicate  the  two  intersections  of  line 
segment  ab  and  arc  C.  The  shoulder  can  reach  the  line 
segment  kr.  Therefore,  line  segment  kr  is  shifted  out  a 
distance  M to  the  line  segment  js.  The  corners  of  the 
expanded  object  are  now  defined  sequentially  by  the  points 
j , s , g and  h . 

Case  6:  Points  a and  b are  both  outside  arc  C.  Side 

ab  intersects  arc  C.  Point  0 and  c are  on 
the  same  side  of  line  segment  ab  (see  figure 
2.10). 

In  this  case,  the  shoulder  will  not  touch  side  ab. 
Therefore,  the  sides  ab  and  be  are  shifted  out  a distance  N 
to  the  line  segments  de  and  ef.  Corners  g and  h are  then 
determined  as  in  Case  1.  The  corners  of  the  expanded  object 
are  now  defined  by  the  points  g and  h. 

The  technique  described  above  is  applied  to  expand  the 
sides  of  each  of  the  objects.  Finally,  after  all  the  new 
vertices  of  each  object  have  been  determined,  the  heights  of 
these  vertices  are  shifted  vertically  a distance  V (see 
figure  2.1.2)  in  order  to  avoid  collision  when  the  arm  lies 
horizontally  over  the  top  of  the  object.  These  new  vertices 
determine  the  shape  and  size  of  the  new  expanded  object. 
Figure  2.11  illustrates  the  shape  of  an  object  before  and 


after  expansion. 
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Figure  2.11  The  shape  of  an  object  before 
and  after  expansion 
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2 . 2 The  Safe  Angular  Range  of  Joints  2 and  3 

The  expansion  technique  described  in  the  previous 
section  simplifies  the  collision  avoidance  problem  between 
an  object  and  a solid  robot  manipulator  with  links  of 
rectangular  cross  sections  by  expanding  the  object  and  by 
reducing  the  links  to  line  segments  with  zero  cross  section. 
When  a line  segment  does  not  touch  the  expanded  object,  the 
corresponding  solid  link  will  not  touch  the  original  object. 
However,  the  shoulder  and  arm  of  the  articulated  robots  are 
always  inclined.  The  sections  of  the  shoulder  and  arm  cut 
by  the  horizontal  plane  at  the  same  height  as  the  object  are 
somewhat  larger  than  their  cross  sections  (see  figure  2.1, 
P'R  > PR  and  O'S  > QS).  Because  all  objects  have  been 
expanded  based  on  the  size  of  the  two  cross  sections  for 
interference  avoidance,  the  safe  angular  ranges  of  joints  2 
and  3 may  be  less  than  the  joint  limitation.  This  is 
evident  by  considering  figure  2.1  where  9 and  (j)  denote  the 
angles  of  the  shoulder  and  arm  axes  measured  relative  to  the 
horizontal.  When  the  distance,  P'R  - PR  VcotG  and  O'S  - 
QS  >_  Vcotcf),  the  shoulder  and  arm  will  collide  respectively 
with  the  object.  It  follows  that  the  safe  angular  ranges  of 
joints  2 (9)  and  3 (4>)  need  to  be  derived. 

Case  1:  The  safe  angular  range  of  joint  2. 

From  above  discussion,  the  clearance  obtained  from  the 
expansion  of  the  height  of  the  object  should  be  greater  than 
the  distance  of  P'R  - PR,  see  figure  2.12,  and  therefore 
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Figure  2.12  P'R-PR  < Vcote 


Figure  2.13  P'R-PR  > VcotG 
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Vcot9  P'  R - PR  (2.7) 

Substituting  P'R  = PR/sin©  in  equation  (2.7)  and 


simplifying  the  equation  yields 

Vcos©  + PRsin©  PR  (2.8) 

which  can  be  expressed  in  the  form 

Lsin(0+(f))  >_  PR  (2.9) 

where  L = (V^  + PR^)^/^,  cj)  = sin"^(V/L)  (2.10) 

Solving  equation  (2.9)  for  9 gives 

9 = sin~^(PR/L)  - sin"^(V/L)  (2.11) 


-3 

The  values  of  V and  PR  for  the  T -776  robot  are  12.1 
and  4.2  inches  respectively.  Therefore,  the  minimum  value 
for  9 is  42.04  degrees.  When  9 < 42.04,  the  shoulder  must 
be  rotated  upwards  through  an  angle  0 £ iji  £ 180  in  order  to 
avoid  collision  (see  figure  2.13).  The  following 
relationship  can  be  deduced  from  figure  •2.13  by  applying  the 
sine  rule  to  triangle  ABC. 

(H  - T)  / sin©  D 

sin(  n - © - ijl)  sinijj  (2.12) 

where  D = P ' R - PR  - Vcot© 

H is  the  height  of  the  obstacle 
T is  the  offset  of  joint  1 


28 


Solving  equation  (2.12)  for  ijj  yields 

ijl  = cot  ( cote) 

D X sin^e  (2.13) 

Figure  2.14  shows  an  example  in  which  the  shoulder  is 
rotated  upwards  i|i  degrees  to  avoid  collision. 

Case  2:  The  safe  angular  range  for  joint  3. 

The  analysis  is  virtually  identical  to  Case  1 (see 
figure  2.15).  Here, 

Vcot(j)  ^ O'S  - OS  (2.14) 

Substituting  Q'S  = QS/sin(|)  in  equation  (2.14)  and 
simplifying  the  equation  yields 

Vcoscj)  + QSsincj)  >_  QS  (2.15) 

The  solution  for  which  is 

(|)  > sin"^(OS  / L)  - sin"^(V  / L)  (2.16) 

where  L = (V^  + 0S^)^^^ 

"3 

The  magnitude  of  OS  for  the  T -776  robot  is  4.1 
inches  and  from  (2.15)  the  minimum  value  for  (j)  is  -12.7 
degrees.  This  implies  that  there  is  no  restriction  on  the 
value  of  (|)  since  (j)  is  always  greater  than  zero. 

2 . 3 The  Equation  for  an  Archimedes’s  Spiral 


Clearly  there  are  a wide  variety  of  paths  for  the  end 
effector  of  an  articulated  type  robot  moving  between  two 
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Figure  2.14  The  shoulder  is  rotated  upwards 
degrees  to  avoid  collision 
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Figure  2.15  Q ' S-QS  < Vcot© 
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positions,  namely,  a line  segment,  an  arc,  a parabola,  a 
spiral  and  so  on.  Since  the  path  may  pass  through  the  void 
of  the  workspace,  a rectilinear  motion  for  an  articulated 
type  robot  may  not  be  possible.  It  is  also  difficult  for  an 
arc  and  a parabola  to  determine  an  optimum  radius  and 
curvature . 

The  Archimedes's  spiral  is  determined  by  the  initial 
position  I and  goal  position  G of  the  wrist  (see  figure 
2.16)  and  this  will  always  lie  within  the  workspace  of  the 
wrist.  The  parametric  equations  for  the  Archimedes's  spiral 
defined  by  the  positions  I and  G are 

L = L.  + (Lg  - L.)  e / (|)  (2.17) 

H = Hi  + (Hg  - Li)  e / <j)  (2.18) 

where  L,  and  : the  distance  from  the  origin  to 

the  projection  of  points  R,  I and 
G on  the  X-Y  plane. 

H,  and  : the  height  of  points  R,  I and  G. 

(|>:  the  angle  between  planes  GOZ  and  lOZ . 

0:  the  angle  between  planes  ROZ  and  lOZ . 

2 . 4 Identification  of  the  Obstacles 

In  the  algorithm  to  be  developed  here,  it  is  assumed 
that  the  wrist  will  follow  the  spiral  descibed  above 
whenever  it  is  reachable.  However,  when  some  point  on  the 
spiral  is  too  low,  the  shoulder  or  arm  will  not  be  able  to 
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Figure  2.16  The  Archimedes's  spiral  and 
fan- shaped  area 
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avoid  collision.  This  situation  is  illustrated  in  figure 
2.17.  The  moon-shaped  area  is  a cross  section  of  the 
wrist's  reachable  space.  The  hatched  section  is  the  wrist's 
reachable  area  determined  by  the  obstacle  A.  The 
intersection  e of  the  spiral  and  cross  section  is  too  low  to 
be  reached  by  the  wrist  without  collision  between  the  arm 
and  obstacle  A.  In  this  situation,  the  corresponding 
reachable  point  f on  the  lower  bound  (line  segment  ab)  of 
the  reachable  area  and  with  the  same  X coordinate  as  point  e 
is  chosen  as  the  real  reachable  point.  When  there  is  no 
such  corresponding  reachable  point  (for  example,  point  g has 
no  corresponding  reachable  point),  the  reachable  point,  b, 
with  the  greatest  X coordinate  will  be  selected.  When  the 
obstacle  is  too  high  to  pass  over  (see  figure  2.18),  a 
reachable  point  f will  be  determined  by  the  corresponding 
point  e'  which  is  on  the  origin  side  of  the  obstacle  and 
with  the  same  Z coordinate  as  the  point  e.  These  rules  for 
path  planning  restrict  the  wrist's  path  to  lie  within  the 
space  determined  by  the  initial  vertical  plane  (defined  by 
the  initial  point  and  the  Z-axis),  the  final  vertical  plane 
(defined  by  the  target  point  and  the  Z-axis) , and  the 
trajectory  of  the  spiral  with  its  projection  on  the  X-Y 
plane.  Obviously,  when  the  wrist  moves  along  the  path,  the 
shoulder  and  arm  will  always  stay  inside  of  the  same  space. 
This  also  indicates  that  only  an  object  satisfying  the 
following  condition  is  considered  to  be  an  obstacle. 
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Figure  2.17 
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Figure  2.18 
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The  object  must  lie  inside  of  or  intersect  with 


the 

fan-shaped  area  determined 

by 

the  origin  of 

the 

fixed  coordinate  system 

and 

an 

expanded 

projection  of  the  spiral  on 

the 

X- 

Y plane. 

The  expanded  projection  of  the  spiral  on  the  X-Z  plane 
(see  figure  2.16)  is  obtained  simply  by  expanding  the 
projection  of  the  spiral  radially  by  an  amount  equal  to  the 
length  of  the  line  segment  joining  the  wrist  point  to  the 
tip  of  the  end  effector.  This  expansion  is  because  the  end 
effector  is  not  under  consideration  when  the  spiral  is 
defined  and  it  may  touch  the  objects  near  the  spiral. 

The  objects  outside  the  fan-shaped  area  can  thus  be 
neglected.  The  procedure  to  determine  whether  an  object  is 
to  be  considered  as  an  obstacle  or  not  is  now  described. 

1.  The  angular  ranges  of  the  projection  of  the  object 
on  the  X-Y  plane  and  the  fan-shaped  area  corresponding  to 
the  positive  X-axis  are  determined.  In  figure  2.16,  the 
angular  ranges  of  object  F and  the  fan-shaped  area  are 
a to  B and  y to  respectively. 

2.  When  there  is  no  overlap  of  these  two  angular 
ranges,  the  object  is  outside  of  the  fan-shaped  area  and  is 
therefore  not  an  obstacle  (this  is  the  case  for  object  F). 

3.  When  there  is  overlap,  the  intersection  of  the 
expanded  projection  of  the  spiral  and  the  line  segment 
connecting  the  origin  and  each  of  the  bottom  vertices  of  the 
object  is  determined.  If  there  is  a vertex  closer  to  the 
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origin  than  this  intersection,  the  object  intersects  with 
the  fan-shaped  area  and  it  must  be  considered  as  an 
obstacle.  If  no  vertex  is  closer  to  the  origin  than  this 
intersection,  the  object  is  outside  of  the  fan-shaped  area 
and  is  not  considered  as  an  obstacle.  In  figure  2.17,  the 
vertex  a of  object  C is  closer  to  the  origin  than  its 
corresponding  intersection  b.  Hence  object  C is  considered 
to  be  an  obstacle. 

The  algorithm  for  the  expansion  of  objects  and 
reduction  of  the  manipulator  to  line  segments  described  in 
section  2.1  together  with  the  algorithm  for  the 
identification  of  obstacles  described  above  were  applied  to 
the  obstacle  strewn  environment  illustrated  by  figure  2.16. 
In  figure  2.19,  the  resulting  modified  objects  A,  B and  C 
are  considered  as  obstacles  while  objects  D,  E and  F can  be 
removed  from  further  consideration. 
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Figure  2.19  The  expanded  obstacles  and 
manipulator 


CHAPTER  III 


DETERMINATION  OF  REACHABLE  POINTS 
TO  FORM  A COLLISION-FREE  PATH 

After  all  the  obstacles  have  been  identified  as 
discussed  in  Chapter  II,  a series  of  vertical  planes  are 
formed  for  determining  reachable  points.  Each  of  the 
vertical  planes  is  defined  by  the  vertical  Z-axis  and  a top 
vertex  point  of  one  of  the  obstacles.  These  planes  generate 
cross  sections  of  the  obstacles  and  wrist's  reachable  space. 
The  cross  sections  of  the  reachable  space  and  obstacles  are 
mapped  through  rotational  transformations  onto  the  X-Z 
plane.  These  transformations  simplify  the  three-dimensional 
path  problem  to  a two-dimensional  one.  In  this  chapter,  a 
method  to  find  the  lower  bound  of  the  wrist's  reachable 
areas  and  to  determine  reachable  end  effector  points  is 
discussed . 

3 . 1 The  Quicksort  Algorithm 

The  angles  between  the  X-Z  plane  and  the  vertical 
planes  described  above  are  computed.  These  planes  are  sorted 
according  to  the  angles  with  the  X-Z  plane  beginning  with 
the  smallest  angle  and  concluding  with  the  largest  angle. 
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The  sorting  algorithm  used  in  this  paper  is  called 
"quicksort."  The  essence  of  quicksort  is  to  sort  an  array 
A[l],  A[2],...,  A[n]  by  permuting  the  elements  in  the  array 
so  that  for  some  "j",  all  the  records  with  keys  less  than 
some  key  value  "v"  appear  in  A[l],...,  A[j],  and  all  those 
with  keys  "v"  or  greater  appear  in  A [ j +1 ] , . . . , A [ n] . Then 
quicksort  is  applied  recursively  to  A [ 1 ] , . . . , A [ j ] and  to 
A[ j+1] , . . . ,A[n]  to  sort  both  these  groups  of  elements. 
Since  all  keys  in  the  first  group  precede  all  keys  in  the 
second  group,  the  entire  array  will  be  sorted.  Assuming 
that  the  algorithm  recursively  sorts  the  array  S ( 1 ),..., S ( N ) 
of  N items  into  increasing  order,  then  the  steps  of  this 
sorting  are  as  follows, 

1.  [Trivial?]  If  N _<  1 , return.  (N  = 0 means  that  the 
array  is  empty.) 

2.  [Initialization.]  Set  I = 1;  J = N.  (I  moves  right 
and  J moves  left  along  the  array  as  elements  are 
interchanged.  When  I = J,  the  element  at  the  Ith 
position  will  be  greater  than  or  equal  all  elements 
to  the  left  and  less  than  or  equal  all  elements  to 
the  right . ) 

3.  [Move  J?]  IF  S(J)  >_  S(I),  then  J = J - 1 ; go  to  3; 
otherwise,  (interchange)  TEMP  = S ( I ) ; S(I)  = S(J); 
S(J)  = TEMP. 

4.  [Move  I?]  If  I = J,  go  to  6;  otherwise,  1=1+1. 
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5.  [Interchange?]  If  S(I)  < S(J),  go  to  4;  otherwise, 
(interchange)  TEMP  = S(I);  S(I)  = S(J);  S(J)  = 

TEMP;  J = J - 1 ; go  to  step  3. 

6.  [Sort  subproblems.]  Call  the  algorithm  with  input 
array  S ( 1 S ( I-l ) ; Call  the  algorithm  with 
input  array  S ( I+l S ( N ) ; Return. 

An  example  of  the  application  of  this  quicksort 
algorithm  is  illustrated  in  figure  3.1. 

3 . 2 Obstacle  Cross  Sections 

The  obstacles  and  vertical  planes  defined  previously 
are  projected  on  the  X-Y  plane.  The  shapes  of  the 
projections  of  obstacles  are  the  same  as  those  of  their  top 
surfaces  (polygons).  The  projections  of  the  vertical  planes 
are  represented  by  lines.  If  there  is  intersection  between 
any  polygon  and  any  line,  the  corresponding  obstacle  of  the 
polygon  is  cut  by  the  corresponding  vertical  plane  of  the 
line  and  a cross  section  of  this  obstacle  can  be  determined. 
Assume  that  points  a and  b are  the  two  end  points  of  one  of 
the  sides  of  the  polygon. 

1.  If  points  a and  b are  on  the  same  side  of  the  line, 
there  is  no  intersection. 

2.  If  points  a and  b are  on  opposite  sides  of  the 
line,  the  intersection  can  be  found  by  solving  the 
line  equation  and  the  equation  of  the  line  passing 
through  points  a and  b. 
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Figui'e  3.1  Quicksort  algorithm 
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3.  If  one  or  two  vertices  are  on  the  line,  they  are 
themselves  points  of  intersections. 

The  angular  range  of  each  of  the  obstacles 


corresponding 

to 

the 

positive 

X-axis  has  been 

defined  in 

Chapter 

II . 

All 

sides 

of  the 

polygon  are  sequentially 

checked 

against 

each 

of  the 

projection  lines 

within  the 

angular 

range 

of 

the 

corresponding  obstacle. 

Each  two 

sequential  intersections  will  determine  a cross  section  of 
the  obstacle.  Those  cross  sections  are  then  transformed  to 
the  X-Z  plane.  In  general,  this  transformation  is  done  by 
multipling  the  coordinates  of  the  intersections  with  a 
rotation  transformation  matrix.  However,  this  is  easily 
accomplished  by  replacing  the  X coordinate  of  the 
intersections  by  the  square  root  of  the  summation  of  X 
square  and  Y square  while  replacing  the  Y coordinate  by  0 
and  not  changing  the  Z coordinate. 

3 . 3 The  Low  Bound  of  the  Reachable  Area 

As  described  earlier,  an  Archimedes's  spiral  is 
determined  by  the  initial  and  goal  positions  of  the  wrist. 
This  spiral  intersects  with  the  first  cross  section  of  the 
wrist's  reachable  space.  The  intersection,  which  is  called 
the  recommended  reachable  wrist  point,  is  used  to  find  a 
real  reachable  wrist  point.  The  real  reachable  wrist  point 
and  goal  position  will  generate  a new  Archimedes's  spiral. 
The  intersection  of  this  new  spiral  and  the  second  cross 
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section  of  the  wrist's  reachable  space  is  the  new 
recommended  reachable  wrist  point.  This  recommended  point 
is  then  used  to  find  a further  real  reachable  wrist  point 
and  so  on.  These  procedures  are  repeated  until  the  real 
reachable  wrist  point  which  lies  on  the  last  cross  section 
is  determined. 

In  each  of  the  cross  sections  of  the  wrist's  reachable 

space,  the  X and  Z coordinates  of  the  recommended  reachable 

point  are  denoted  by  and  respectively.  The  algorithm 

to  be  developed  will  find  the  lower  bound  of  the  reachable 

area  for  the  wrist  between  the  Z-axis  and  line  X = X and 

e 

determine  a reachable  wrist  point  in  this  reachable  area. 
Thus,  the  shoulder  and  arm  will  stay  within  this  area  and 
any  obstacle  (or  any  cross  section  of  the  obstacle)  outside 
the  area  between  lines  X = 0 and  X = X^  can  be  excluded 
from  consideration.  Figure  3.2  illustrates  that  the 
obstacle  D is  out  of  the  area. 

In  order  to  describe  more  easily  how  the  lower  bound  of 
the  reachable  area  for  the  wrist  is  determined,  a pair  of 
principal  obstacles  is  defined,  one  for  the  shoulder  and  the 
other  for  the  arm. 

1.  The  principal  shoulder  obstacle  is  defined  as  the 
obstacle  first  touched  by  the  shoulder  when  it  is  rotated 
downwards  from  its  vertical  position  (for  example,  the 
obstacle  A in  figure  3.2). 
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Figure  3.2  The  principal  shoulder  and  arm 
obstacles 
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is  defined  as  the 


2.  The  principal  arm  obstacle 
obstacle  first  touched  by  the  arm  when  it  is  rotated 
downwards  from  its  extreme  position  and  when  the  angle  of 
joint  3 equals  the  maximum  joint  limitation  (150  degrees) 
(for  example,  the  obstacle  B in  figure  3.3).  However,  when 
the  shoulder  touches  an  obstacle  first  (see  obstacle  A in 
figure  3.2),  the  arm  is  rotated  downwards  further  until  it 
touches  an  obstacle  (see  obstacle  C in  figure  3.2)  which  is 
defined  as  the  principal  arm  obstacle. 

The  two  principal  obstacles  will  be  used  to  determine 
the  lower  bound  of  the  reachable  area  between  the  origin 
side  of  the  principal  arm  obstacle  and  the  line  X = X^. 

Obviously,  when  there  is  no  obstacle  whose  origin  side 
top  corner  is  within  the  circular  arc  generated  by  joint  3 
(see  figure  3.4),  the  shoulder  cannot  touch  any  obstacle. 
Hence  only  the  collision  problem  between  the  arm  and 
obstacles  needs  to  be  considered.  In  this  situation,  when 
the  highest  obstacle  is  higher  than  the  maximum  height  H 
that  the  joint  3 can  reach,  the  arm  may  only  touch  the 
origin  side  top  corner  of  the  principal  arm  obstacle  (for 
instance,  the  obstacle  C of  figure  3.4).  When  the  height  of 
the  highest  obstacle  is  between  the  maximum  and  minimum 
heights  (H  and  L)  that  joint  3 can  reach,  the  arm  can  lie 
horizontally  on  the  top  of  the  principal  arm  obstacle  and 
may  touch  each  of  the  two  top  vertices  of  the  obstacle  (for 
example,  the  obstacle  B of  figure  3.4).  When  the  highest 
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Figure  3.3  The  principal  arm  obstacle 


4 a 


Figure  3.4  The  relation  between  the 
manipulator  and  obstacles 
St  different  height  ranges 


obstacle  is  lower  than  the  minimum  height  that  joint  3 can 
reach,  the  arm  (not  including  the  tip  of  the  wrist)  will 
only  touch  the  exterior  top  corner  (the  top  corner  furthest 
from  the  origin)  of  the  principal  arm  obstacle  (see  the 
obstacle  A of  figure  3.4).  These  different  conditions 
determine  all  the  Group  1 cases  listed  in  figure  3.5. 

When  there  is  at  least  one  obstacle  whose  origin  side 
top  corner  is  within  the  circular  arc  generated  by  joint  3 
(for  example,  the  obstacle  A and  B of  figure  3.2),  the 
shoulder  may  first  touch  one  of  the  obstacles.  In  this 
situation,  the  collision  problems  between  the  arm  and 
obstacles  and  between  the  shoulder  and  obstacles  must  be 
solved  simultaneously.  The  Group  2 cases  listed  in  figure 
3.5  describe  all  possible  cases  when  there  is  at  least  one 
obstacle  whose  origin  side  top  corner  is  inside  the  circular 
arc  generated  by  joint  3 (the  trajectory  of  joint  3). 

The  determination  of  the  lower  bound  of  the  wrist's 
reachable  area  and  the  reachable  wrist  point  is  now 
discussed. 

Group  1:  There  is  no  obstacle  whose  origin  side  top 
corner  is  within  the  trajectory  of  joint  3. 

In  this  case,  the  shoulder  will  not  touch  any  obstacle. 
Hence  it  is  not  necessary  to  consider  the  collision  between 
the  shoulder  and  obstacles.  There  are  three  cases  which 
depend  on  the  height  of  the  obstacles  and  are  considered 
separately. 
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*e 


the  mini-Tium  angle  of  joint  2 when  the  shoulder 
does  not  touch  any  obstacle. 


Point  Q;  the  lowest  position  of  joint 
does  not  touch  any  obstacle. 


3 when  the  shoulder 


Point  U: 


the  position  of  joint  3 when  the  wrist  is  located  at 
the  right  top  point  of  the  principal  ar."  obstacle. 


Point  T:  the  intersection  of  the  trajectory  of  jci 
the  line  passing  through  point  Q and  the 
point  of  the  principal  arr;  obstacle. 


Figure  3.5  The  list  of  all  cases 
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Case  1.1:  At  least  one  obstacle  is  higher  than  the 


maximum  height  H which  joint  3 can  reach. 


In  figure  3.6,  the  obstacles  A and  B are  higher  than 
height  H.  Points  P and  0 are  the  positions  of  the  centers 
of  joints  2 and  3 respectively.  The  shoulder  and  arm  are 
represented  by  line  segments  PQ  and  OS  (Point  S denotes  the 
position  of  the  wrist) . 

In  this  case,  the  reference  angle  of  an  obstacle  higher 
than  H is  defined  as  the  angle  between  the  shoulder  and 
horizontal  (PI)  when  the  angle  cf)  of  joint  3 is  equal  to  the 
maximum  joint  limitation  (150  degrees)  and  when  the  arm 
touches  the  origin  side  top  corner  of  the  obstacle.  The 
principal  arm  obstacle  is  defined  as  the  obstacle  which 
satisfies  the  following  two  conditions. 

1.  The  obstacle  is  higher  than  the  height  H. 

2.  The  reference  angle  of  the  obstacle  is  greater  than 


In  figure  3.6,  the  principal  arm  obstacle  is  A.  Point 
S denotes  the  reachable  wrist  po4nt  with  the  greatest  X 
coordinate.  The  X and  Z coordinates  of  point  S can  be 
derived  through  the  geometric  relation  between  the  links  and 
obstacle  A. 


those  of  other  obstacles. 


Pa  = [ ( Z - PO)^  + X 

d a 


(3.1) 


Pa 


PQ 


s in  a 


(3.2) 
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Figure  3.6  At  least  one  obstacle  is 

higher  than  the  max  height 
H that  joint  3 can  reach 
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where  X,  and  are  the  coordinates  of  point  a. 

a a 

Inserting  equations  3.1  into  3.2,  the  value  of  is 
obtained.  Then  the  angle  0 can  be  calculated  by 

-1  ^a  ~ 

0 = n-(})-a  + tan  ( ) 

(3.3) 

Ci 

The  coordinates  of  point  S are  derived  by 

Xg  = PQcose  + OScos(e  + (j)  - n ) (3.4) 

Zg  = OSsin©  + QSsin(0  + cj)  - n ) (3.5) 

The  locus  of  the  lowest  reachable  points  between  the 
lines  X = X and  X = Xj,  is  a sixth  degree  polynoininal 
(see  APPENDIX  A),  and  the  time  required  to  compute  the  roots 
using  a numerical  method  will  cause  a noticeable  increase  in 
the  response  time.  Because  of  this,  the  line  segment  aS  is 
chosen  to  be  the  lower  bound  of  the  reachable  area  between 
the  lines  X = X and  X = X„.  In  figure  3.6,  the  hatched 

3.  V j 

section  denotes  the  reachable  area  for  the  wrist.  Hence, 
when  the  recommended  reachable  point  is  inside  this  area,  it 
is  a real  reachable  wrist  point. 

When  the  recommended  reachable  point  is  on  the  exterior 
side  (the  side  futhest  from  the  origin)  of  the  line  X = 
Xg,  the  real  reachable  point  with  the  greatest  X 
coordinate,  point  S,  will  be  selected  as  the  reachable  wrist 
point  in  this  cross  section.  When  the  recommended  reachable 
point  is  located  between  lines  X = X^  and  X = Xg  and 
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below  the  line  segment  aS,  the  corresponding  point  on  line 
segment  aS  and  with  the  same  X coordinate  as  the  recommended 
reachable  point  will  be  selected. 

A special  situation  in  this  case  is  when  there  is  an 
obstacle  whose  origin  side  top  corner  is  outside  the  upper 
outer  bound  of  the  cross  section  of  the  wrist's  reachable 
space  (see  obstacle  A of  figure  2.18).  In  this  case,  the 
reachable  area  for  the  wrist  will  be  limited  between  the 
Z-axis  and  the  origin  side  of  this  obstacle,  and  the  wrist 
can  only  move  through  this  reachable  area.  The  lower  bound 
of  this  reachable  area  is  determined  by  the  obstacles  within 
the  area  between  the  Z-axis  and  the  origin  side  of  the 
obstacle  A.  A reachable  wrist  point  is  determined  based  on 
the  lower  bound  of  the  reachable  area.  In  figure  2.18,  the 
reachable  wrist  point  f is  determined  by  the  corresponding 
point  e'  which  is  on  the  origin  side  of  the  obstacle  A and 
with  the  same  Z coordinate  as  the  recommended  reachable 
point  e. 

Case  1.2:  At  least  one  obstacle  is  higher  than  the 
minimum  height  which  joint  3 can  reach. 

Figure  3.7  shows  an  example  of  this  case  for  which  L is 
the  minimum  height  which  joint  3 can  reach.  The  principal 
arm  obstacle  A and  the  reachable  point  with  the  greatest  X 
coordinate,  S,  are  determined  as  in  Case  1.1.  Point  R 


denotes  the 

position 

of 

the 

wrist  when  the  arm 

lies 

horizontally 

on  the 

top 

of 

the  highest  obstacle  A. 

The 
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Figure  3.7  At  least  one  obstacle  is 
higher  than  the  minimum 
height  that  joint  3 can 
reach 
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circular  arc  generated  by  joint  3 and  the  horizontal  line 

through  point  R intersect  in  point  0'(Xg,,ZQ,).  The  X 

coordinate  of  point  R is  thus  given  by  , + O'R, 

K y 

where  O'R  is  the  length  of  the  arm. 

The  loci  of  the  lowest  reachable  points  between  the 
lines  X = 3nd  X = Xj^  and  between  the  lines  X = X^ 
and  X = Xg  are  a pair  of  sixth  degree  polynominals . These 
are  replaced  by  the  line  segments  bR  and  RS  to  reduce  the 
response  time. 

When  the  recommended  reachable  point  (for  example, 

point  g)  is  on  the  exterior  side  of  the  line  X = Xg,  point 

S which  has  the  maximum  X coordinate  will  be  selected  as  the 

reachable  wrist  point.  When  the  recommended  reachable  point 

(for  example,  point  e)  is  located  between  the  lines  X = X, 

b 

and  X = Xg  and  lower  than  the  lower  bound  of  the  reachable 
area,  the  point  on  the  lower  bound  and  with  the  same  X 
coordinate  as  the  recommended  reachable  point  will  be 
selected  as  the  reachable  wrist  point. 

Case  1.3:  All  obstacles  are  lower  than  the  minimum 

height  L which  joint  3 can  reach. 

In  figure  3.8,  point  Q denotes  the  lowest  position  of 
joint  3.  The  reference  angle  0 of  an  obstacle  now  is 
defined  as  the  angle  between  the  horizontal  and  the  line 
passing  through  point  Q and  the  exterior  top  corner  of  the 
obstacle.  The  principal  arm  obstacle  is  therefore  the 
obstacle  which  has  the  smallest  reference  angle  and  this  is 
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Figure  3.8  Point  U lies  inside  arc  TQ 
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clearly  the  obstacle  A,  Point  U denotes  the  position  of 
joint  3 when  the  wrist  is  located  at  point  b (the  exterior 
top  corner  of  the  principal  arm  obstacle  A) . Point  T 
denotes  one  of  the  two  intersections  of  line  Qb  and  the 
circular  arc  generated  by  joint  3 (the  other  intersection  is 
point  0).  The  relative  positions  among  points  T,  0 and  U 
determine  the  shape  of  the  lower  bound  of  the  reachable  area 
between  lines  X = and  X = Xg.  There  are  three 

subcases  which  must  be  considered. 

Case  1.3.1:  Point  U lies  inside  the  arc  TQ  (see  figure 

3.8)  . 

Here,  point  S denotes  the  intersection  of  line  Qb  and 
the  lower  outer  bound  of  the  cross  section  of  the  wrist's 
reachable  space  or  the  extreme  wrist  point  when  the  arm 
touches  the  exterior  top  corner  of  the  principal  arm 
obstacle  A.  The  locus  of  the  lowest  reachable  wrist  points 
between  lines  X = and  X = Xg  lies  below  the  line 

segment  bS  and  for  efficient  computation  is  replaced  by  the 
line  segment  bS. 

When  the  X coordinate  of  the  recommended  reachable 
point  is  greater  than  Xg,  it  is  a real  reachable  point. 
When  the  recommended  reachable  point  is  located  between 
lines  X = Xj^  and  X = Xg  and  lower  than  the  lower  bound 
bS,  the  point  on  line  segment  bS  and  with  the  same  X 
coordinate  as  the  recommended  reachable  point  will  be 
selected  as  the  reachable  wrist  point. 
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Case  1.3.2:  Points  T and  0 are  coincident  (see  figure 

3.9)  . 

In  this  case,  the  locus  of  lowest  reachable  points 
between  lines  X = and  X = Xg  lies  above  the  line 

segment  bS.  Because  of  this,  the  locus  is  replaced  by  the 
line  segment  bR,  where  R denotes  the  point  of  intersection 
of  the  lines  Ub  and  X = Xg . The  reachable  wrist  point  is 
determined  by  appling  the  algorithm  developed  in  Case  1.3.1. 

Case  1.3.3:  Point  U lies  outside  the  arc  TQ  (see 

figure  3.10). 

Here,  point  V denotes  the  position  of  the  wrist  when 
joint  3 is  located  at  position  T and  when  the  arm  touches 
the  exterior  top  corner  b of  the  principal  arm  obstacle  A. 
The  locus  of  the  lowest  reachable  points  between  lines  X = 
Xj^  and  X = X^  lies  above  the  line  segment  bS  while  the 
locus  of  lowest  reachable  points  between  lines  X = X and 

V 

X=  Xg  lies  below  the  line  segment  bS.  Because  of  this, 
these  loci  are  replaced  respectively  by  line  segments  bR  and 
VS  as  the  lower  bounds  of  the  reachable  area,  where  R 
denotes  the  point  of  intersection  of  the  lines  Ub  and  X = 
X^.  The  reachable  wrist  point  is  determined  by  appling 
the  algorithm  developed  in  Case  1.3.1. 

Group  2:  There  is  at  least  one  obstacle  whose  origin 

side  top  corner  is  within  the  trajectory  of 
joint  3 . 
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Figure  3.9  Points  P and  Q are  coincident 
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Figure  3.10  Point  U lies  outside  arc  TQ 
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In  this  case,  the  reference  angle  <j)  of  an  obstacle  is 
defined  as  the  angle  between  the  positive  X-axis  and  the 
line  segment  passing  through  joint  2 and  the  origin  side  top 
corner  of  the  obstacle.  The  principal  shoulder  obstacle  is 
defined  as  the  obstacle  which  satisfies  the  following  two 
conditions . 

1.  The  origin  side  top  corner  of  the  obstacle  is 
within  the  circular  arc  generated  by  joint  3. 

2.  The  reference  angle  of  the  obstacle  is  greater 
than  that  of  other  obstacles. 

In  figure  3.11,  the  principal  shoulder  obstacle  is  A, 
because  (|)  > ©.  There  are  two  cases  which  must  be  considered 
separately. 

Case  2.1:  The  reference  angle  of  the  principal 

shoulder  obstacle  is  less  than  the  minimum 
allowable  angle  of  joint  2. 

In  this  case,  the  shoulder  will  not  collide  with  any 
obstacle.  This  case  is  analogous  to  Group  1 and  thus  the 
procedures  already  developed  for  determining  the  lower  bound 
of  the  reachable  area  and  the  reachable  wrist  point  are 
appl ied . 

Case  2.2:  The  reference  angle  of  the  principal 

shoulder  obstacle  is  greater  than  the 
minimum  allowable  angle  of  joint  2. 

Figure  3.11  illustrates  an  example  of  this  case  for 
which  (j)  > 30.  In  this  case,  the  shoulder  may  touch  the 
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Figure  3.11  At  least  one  obstacle  is  higher 
than  the  maximum  height  H that 
joint  3 can  reach 
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principal  obstacle.  There  are  four  subcases  which  are 
determined  by  the  height  of  the  obstacles  whose  exterior  top 
corners  are  outside  of  the  trajectory  of  joint  3. 

Case  2.2.1:  At  least  one  obstacle  is  higher  than  the 

maximum  height  H which  joint  3 can  reach 
( see  figure  3.11). 

Point  0 is  used  to  denote  the  position  of  joint  3 when 
the  shoulder  touches  the  origin  side  top  corner  of  the 
principal  shoulder  obstacle  A.  Point  O'  denotes  the 
position  of  joint  3 when  the  arm  touches  the  origin  side  top 
corner  a of  the  principal  arm  obstacle  and  when  the  angle  of 


joint  3, 

eguals 

the  maximum  allowable  angle. 

When  the  Z 

coordinate  of 

point 

0 is  greater  than 

that  of 

point 

0'  , 

point  0 

is 

used 

to  determine  the 

lower  bound  of 

the 

reachable 

area.  Otherwise,  point  Q is 

replaced 

by  point 

0' 

to  determine 

the 

lower  bound  of  the 

reachable 

area. 

This 

ensures 

that 

the 

shoulder  will  not 

collide 

with 

the 

obstacles.  In  the  following  cases,  the  7,  coordinate  of 
point  0 is  assumed  to  be  greater  than  that  of  point  O'. 

After  point  0 has  been  chosen,  the  reachable  point  with 
the  greatest  X coordinate,  S,  can  be  determined  by 
considering  the  coordinates  of  points  0 and  a.  As  in  Case 
1.1,  the  line  segment  aS  is  chosen  as  the  lower  bound  of  the 
reachable  area  between  lines  X = X X = X 

s • 

When  the  X coordinate  of  the  recommended  reachable 
point  (for  example,  point  g)  is  less  than  that  of  point  S, 
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point  S is  selected  as  the  real  reachable  wrist  point.  When 
the  recommended  reachable  point  (for  example,  point  e)  is 
located  between  lines  X = and  X = X„  and  below  the 
line  segment  aS,  the  point  f on  line  segment  aS  and  with  the 
same  X coordinate  as  point  e is  selected. 

Case  2.2.2:  At  least  one  obstacle  is  higher  than  the 

height  of  point  0 (see  figure  3.12). 

In  this  case,  the  reachable  wrist  point  with  greatest  X 
coordinate  S is  determined  as  Case  2.2.1.  Point  R denotes 
the  position  of  the  wrist  when  the  arm  lies  horizontally  on 
the  highest  obstacle  B.  The  procedures  for  determining  the 
X coordinate  of  point  R and  the  reachable  wrist  point  are 
the  same  as  those  developed  in  Case  1.2. 

Case  2.2.3:  All  obstacles  whose  exterior  top  corners 

are  outside  the  trajectory  of  joint  3 are 
lower  than  the  height  of  point  Q.  (see 
figure  3.13) 

Point  V denotes  the  reachable  wrist  point  with  the 
greatest  X coordinate.  The  X coordinate  of  point  V eguals 
+ OV,  where  QV  is  the  length  of  the  arm.  Point  S 
denotes  the  wrist  position  when  the  arm  touches  the  exterior 
top  corner  of  the  principal  arm  obstacle.  The  locus  of  the 
lowest  reachable  points  between  points  b and  S is  an  order 
six  polynomial  which  has  been  discussed  in  Case  1.3. 

When  the  X coordinate  of  the  recommended  reachable 
point  is  greater  than  X^,  point  V is  selected  as  the 


66 


Figure  3.12  At  least  one  obstacle  is  higher 
than  the  height  of  point  Q 
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Figure  3.13  No  obstacle  is  higher  than 
the  height  of  point  Q 
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reachable  wrist  point.  When  the  recommended  reachable  point 
is  located  between  lines  X = Xg  and  X^  and  below  line 
segment  SV,  the  reachable  point  on  line  segment  SV  and  with 
the  same  X coordinate  as  the  recommended  point  is  selected. 
When  the  recommended  reachable  point  is  located  between 
lines  X = Xj^  and  X = Xg,  the  reachable  wrist  point  is 
determined  by  appling  the  algorithm  developed  in  Case  1.3. 

Case  2.2.4:  There  is  no  obstacle  whose  exterior  top 

corner  is  outside  the  trajectory  of  joint 
3 (see  figure  3.14). 

In  this  case,  the  lower  bound  of  the  reachable  area  is 

the  arc  whose  center  is  point  Q and  whose  radius  equals  the 

length  of  the  arm.  The  wrist  point  with  the  greatest  X 

coordinate,  V,  is  determined  using  the  algorithm  developed 

in  Case  2.2.3.  When  the  X coordinate  of  the  recommended 

reachable  point  is  greater  than  point  V is  selected  as 

the  reachable  wrist  point.  When  the  recommended  reachable 

point  is  located  between  lines  X = X^  and  X = 0 and  lower 

than  the  lower  bound  arc  of  the  reachable  area,  the 

intersection  f of  line  X = X and  the  lower  bound  arc  is 

e 

selected  as  the  reachable  wrist  point. 

3 . 4 Modification  of  the  Path 

Employing  the  above  analysis,  the  reachable  wrist  point 
on  each  of  the  cross  sections  can  be  determined.  It  will 
now  be  proved  that  the  line  segment  joining  any  pair  of 
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Figure  3.14  There  is  no  obstacle  whose 

exterior  top  corner  is  outside 
the  trajectory  of  joint  3 
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consecutive  reachable  points  is  collision-free  and  hence  the 
path  that  follows  the  sequence  of  line  segments  between 
successive  points  is  a collision-free  oath  for  the  wrist. 
There  are  two  cases  which  must  be  considered.  The  first  is 
a line  segment  which  joins  a pair  of  consecutive  reachable 
points  with  the  shoulder  in  contact  with  an  obstacle.  The 
second  is  a line  segment  joining  a pair  of  consecutive 
reachable  points  with  the  arm  in  contact  with  an  obstacle. 

1.  The  shoulder  in  contact  with  an  obstacle. 

In  figure  3.15,  the  circular  arc  C is  the  locus  of  the 
wrist's  position  when  the  shoulder  touches  an  arbitrary 
obstacle  A at  origin  side  top  corner  a.  The  upper  and  lower 
bound  of  arc  C,  u and  v respectively,  are  determined  by  the 
joint  limit.  Clearly,  when  the  wrist  is  located  inside  the 
shaded  area,  the  shoulder  cannot  touch  the  obstacle  A. 

In  figure  3.16.1,  points  S and  T are  used  to  denote  the 
positions  of  the  wrist  when  the  shoulder  touches  an 
arbitrary  obstacle  A at  points  a and  b respectively.  Points 
U and  V indicate  the  corresponding  positions  of  joint  3. 
When  the  wrist  moves  rectilinearly  from  S to  T,  the  joint  3 
will  follow  the  curve  UV.  This  curve  is  an  arc  on  the 
sphere  whose  center  is  point  P (the  center  of  joint  2)  and 
whose  radius  equals  the  length  of  the  shoulder.  Figure 
3.16.2  illustrates  the  projection  of  the  arc  on  the  X-Y 
plane.  Clearly  this  projection  is  always  a concave  toward 
the  origin  0 and  thus  the  surface  generated  by  the  loci  of 
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Figure  3.15  The  lower  bound  of  the 
reachable  wrist  area 
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Figure  3.16.1  The  locus  of  joint  3 


Y 


Figure  3.16.2  The  projection  of  figure 

3.16.1  on  the  X-Y  plane 
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the  wrist  between  two  adjoining  cross  sections  (for  example, 
the  planes  PUS  and  PVT)  is  a portion  of  an  torus  and  concave 
upwards  as  illustrated  by  figure  3.17.  Further,  all  the 
points  on  any  line  segment  joining  any  pair  of  points  T and 
S on  the  surface  must  lie  above  the  surface.  It  follows 
that  the  shoulder  cannot  touch  obstacle  A during  the 
rectilinear  motion  of  the  wrist  from  position  S to  T. 

2.  The  arm  in  contact  with  an  obstacle. 

There  are  three  cases  which  are  depended  on  the  height 
of  the  obstacle.  Each  of  the  cases  is  discussed  separately. 

Case  1;  The  height  of  the  obstacle  is  lower  than  the 
minimum  height  that  the  joint  3 can  reach. 

In  figure  3.18,  points  S and  T denote  the  lowest 
positions  of  the  wrist  when  the  arm  touches  an  arbitrary 
obstacle  A at  the  top  corners  a and  b respectively.  Points 
U and  V denote  the  positions  of  the  center  of  joint  3.  Line 
segments  aS  and  bT  are  thus  the  lower  bounds  of  the  wrist's 
reachable  area  in  the  cross  sections  RU3  and  RVT 
respectively.  When  the  wrist  moves  from  position  S to  T and 
along  the  path  generated  by  the  lowest  reachable  points,  the 
locus  UV  generated  by  the  center  of  joint  3 is  an  arc  on  the 
horizontal  circle  C whose  center  is  point  O and  whose  radius 
equals  Lcos9  (see  figure  3.19,  0 = 30  degrees). 

In  figure  3.18,  the  line  segment  ef  is  tangent  to  the 
circle  C and  parallel  to  the  line  segment  ab  and  plane  P 
passes  through  the  parallel  line  segments  ab  and  ef. 


74 


Figure  3.17  The  surface  generated  by 
the  lower  bounds  of  the 
reachable  wrist  area 
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Figure  3.18  The  relation  between  the  locus  of 

joint  3 and  side  ab  of  the  obstacle  A 


d:  the  distance  from  V to  line  ef 


Figure  3.19  The  relation  between  plane  P 
and  points  V and  T 
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Clearly,  the  distance  from  point  T (S)  to  plane  P is 
proportional  to  the  distance  from  point  V (U)  to  line  ef 
(see  figure  3.19).  Figure  3.20  shows  three  possible 
relative  positions  between  the  circular  arc  UV  and  the 
tangent  ef.  In  each  case,  a surface  which  is  generated  by 
the  lower  bounds  of  the  wrist's  reachable  area  is  always 
concave  toward  the  positive  Z-axis.  Therefore,  the  line 
segment  connecting  any  point  Q on  line  segment  aS  and  any 
point  R on  line  segment  bT  is  above  the  surface,  and  the  arm 
will  not  touch  side  ab  during  rectilinear  motion  of  the 
wrist  from  position  Q to  R. 

Case  2:  The  height  of  the  obstacle  is  between  the 

maximum  and  minimum  heights  that  the  joint 
3 can  reach. 

In  this  case,  the  lower  bound  of  the  wrist's  reachable 
area  is  a horizontal  line  segment  at  the  same  height  as  the 
obstacle  (for  example,  the  lower  bound  bR  of  figure  3.7). 
Therefore,  the  surface  determined  by  the  lower  bounds 
between  two  seguential  cross  sections  is  horizontal,  and  the 
wrist  can  move  rectilinearly  between  two  consecutive 
reachable  points  without  any  collision. 

Case  3:  The  height  of  the  obstacle  is  higher  than  the 

maximum  height  that  the  joint  3 can  reach. 

In  figure  3.21,  points  S and  T denote  the  positions  of 
the  wrist  when  the  arm  touches  the  obstacle  A at  top  corners 
a and  b respectively.  When  the  wrist  moves  from  position  S 
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Figure  3.20  The  three  surfaces  generated  by 
the  lower  bound  of  the  reachable 
wrist  area  when  arc  UV  in  different 
positions 
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S 


Figure  3.21  The  obstacle  A is  higher  than 
the  maximum  height  that  joint 
3 can  reach 
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to  T and  along  the  extreme  path,  the  surface  generated  by 
the  lower  bounds  of  the  reachable  area  is  convex  towards  the 
positive  Z-axis.  Hence,  the  wrist  cannot  move  rectil inearly 
between  two  consecutive  reachable  points  without  any 


collision . 

However,  the 

point 

a is  closer 

to  the  Z-axis 

than  any 

other  point  on 

line 

segment  ab. 

The  plane 

determined 

by  the  lower 

bound 

aS  and  line 

segment  ab  is 

above  the  surface  generated  by  the  lower  bounds  of  each  of 
the  cross  sections.  Therefore,  if  the  reachable  wrist  point 
T is  moved  upwards  on  this  plane,  the  wrist  can  move 
rectilinearly  between  the  two  consecutive  reachable  points  S 
and  T without  any  collision. 

Finally,  consider  a pair  of  seguential  reachable  wrist 
points  a and  b and  one  of  them  (in  this  example,  a)  is  lower 
than  the  obstacle  while  the  remaining  wrist  point,  b,  is 
above  the  obstacle  (see  figure  3.22).  Clearly,  the  wrist 
point  may  collide  with  the  obstacle  when  moving  along  the 
line  segment  ab.  This  type  of  collision  can  be  avoided  by 
moving  the  lowet  point  a vertically  to  point  a'  onto  the 
horizontal  plane  through  the  top  of  the  obstacle. 

In  general,  the  path  which  seguentially  connects  all 
the  reachable  wrist  points  is  not  a smooth  one.  Figure  3.23 
shows  the  ordinates,  a,b,...,r,  of  the  path  at  cross 
sections  1,2,..., 18  for  the  example  shown  in  figure  2.16. 
It  is  desirable  to  shorten  and  smooth  this  path.  From  the 
figure,  it  can  be  seen  that  the  path  segment  mp  is 
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Figure  3.22  Path  segment  ab  intersects  with 
the  obstacle 
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The  second  modified  path 
■ The  third  modified  path 


Figure  3.23  The  initial  and  modified  paths 
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horizontal.  Hence  it  passes  over  the  top  of  some  obstacle 
and  the  ordinates  1 and  q are  thus  moved  to  the  positions  1’ 
and  q'  to  avoid  collision.  Furthermore,  when  the  slope  of  a 
line  segment  is  greater  than  that  of  the  previous  line 
segment  (for  example,  slope  i-j  > h-i),  the  path  chosen  is 
simply  a line  segment  joining  points  h and  j. 

The  reachable  end  effector  points  can  be  determined 
from  the  reachable  wrist  points  by  considering  the  length 
and  orientation  of  the  end  effector.  Because  the  end 
effector  may  intersect  with  some  obstacle,  it  is  necessary 
to  check  for  possible  intersections.  This  is  easily 
accomplished  by  projecting  the  obstacle  on  the  X-Y  plane. 
Figure  3.24  illustrates  the  projection  of  an  obstacle  in  the 
X-Y  plane.  The  vertices  of  which  are  labeled  a,  b,  c,  d and 
e.  Consider  that  point  P is  the  projection  of  either  end 
points  of  the  end  effector  (each  must  be  tested  seperately) . 
Clearly,  for  this  case,  the  summation  of  the  angles  aPb , 
bPc,  etc,  taken  in  order  is  27L  radian.  There  will  be 
collision  if  the  Z coordinate  of  P is  less  than  the  height 
of  the  obstacle.  Hence  collision  is  avoided  by  raising  the 
end  effector  above  the  obstacle.  When  point  Q is  the 
projection  of  either  end  points  of  the  end  effector,  the 
summation  of  the  angles  aQb , bQc , etc.,  taken  in  order  is 
zero  and  there  will  be  no  collision. 

Using  the  above  procedure,  the  trajectory  which  starts 
from  the  initial  position,  and  sequentially  connects  all  the 
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Figure  3.24 


The  relation  between  an  obstacle 
and  the  end  point  of  the  end 
effector 
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reachable  end  effector  points,  and  which  ends  at  the  goal 
position  can  be  modified  into  a smooth  collision-free  path. 


CHAPTER  IV 


APPLICATION  TO  OTHER  TYPE  ROBOTS 


Currently,  most  industrial  robots  operate  with  four 
types  of  coordinate  frames:  the  articulated  type,  the  polar 
type,  the  cylindrical  type  and  the  Cartesian  type.  The 
algorithm  developed  in  the  previous  chapters  provides  an 


efficient  way 

to  determine  a 

collision-free 

path  for 

the 

articulated 

type  robot. 

This  chapter 

discusses 

the 

application  of  this  algorithm  to  other  type  robots. 

4 . 1 The  Polar  Type  Robot 

The  main  difference  between  the  articulated  and  polar 
type  robots  is  that  the  third  joint  of  the  polar  type  robot 
is  prismatic  (see  figure  4.1).  The  prismatic  joint  always 
has  two  parts,  a shoulder  of  fixed  length  and  an  extendable 
arm.  Both  have  the  same  orientation.  The  procedures  for 
determining  a collision-free  path  for  this  type  robot  are 
similar  to  those  already  developed  for  the  articulated  type 
robot . 

1.  In  figure  4.2,  the  polygon  labeled  a,  b,  c and  d in 
a counterclockwise  sense  is  the  projection  of  an  arbitrary 
object  A on  the  X-Y  plane.  The  arc  C with  center  0 is  the 
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The  articulated  type  robot 


The  extendable  arm 


The  polar  type  robot 

Figure  4.1  The  articulated  type  and 
polar  type  robots 
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The  original  object  a-b-c-d 
The  expanded  object  e-f-b-g-h-i 

Ob  X ba  < 0,  side  ab  unreachable 

Oc  X cb  < 0,  side  be  unreachable 

Od  X dc  > 0,  side  cd  reachable 

Oa  X ad  > 0,  side  da  reachable 

N and  M have  been  defined  in  Chapter  II 


Figure  4.2  The  expansion  of  an  obstacle 
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intersection  of  the  plane  through  the  top  of  the  object  A 
with  the  surface  generated  by  the  extreme  point  of  the 
shoulder.  When  Ob  x ba  < 0,  the  shoulder  or  arm  will  touch 
point  a first  and  hence  cannot  touch  any  other  points  on  the 
line  segment  ab.  However,  when  Ob  x ba  0 , the  shoulder  or 
arm  can  touch  all  points  on  the  line  segment  ab.  There  are 
three  cases  which  must  be  considered  when  the  vertices  of 
objects  are  expanded. 

a.  When  a pair  of  sequential  sides  of  an  object  are 
both  not  reachable  by  the  shoulder  and  arm,  the  vertex  of 
intersection  is  chosen  as  the  vertex  of  the  expanded  object. 
In  figure  4.2,  the  shoulder  and  arm  cannot  reach  sides  ab 
and  be  and  the  vertex  b is  chosen  as  a vertex  of  the 
expanded  object. 

b.  When  one  of  the  two  sequential  sides  is  reachable 
(for  example,  side  cd  is  reachable  while  side  be  is  not 
reachable),  the  new  vertex  (point  g)  corresponding  to  the 
reachable  side  can  be  determined  using  the  algorithm 
developed  in  Chapter  II. 

c.  When  both  of  the  sequential  sides  are  reachable 
(for  example,  sides  cd  and  da),  the  two  new  vertices  (points 
h and  j)  are  again  determined  using  the  algorithm  developed 
in  Chapter  II. 

After  all  the  new  vertices  have  been  found,  they  are 
shifted  vertically  by  half  the  thickness  of  the  shoulder 
when  the  shoulder  can  touch  the  top  of  the  original  object; 
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otherwise,  they  are  shifted  vertically  half  the  thickness  of 
the  arm.  Following  this,  the  shoulder  and  arm  can  be 
considered  together  as  an  extendable  line  segment  (which 
will  be  called  the  link)  and  the  safe  angular  range  for 
joint  2 can  be  derived  as  described  in  Chapter  II. 

2.  An  Archimedes's  spiral  is  determined  by  the  initial 
and  goal  positions  of  the  wrist.  The  projection  of  this 
spiral  on  the  X-Y  plane  and  the  origin  of  the  fixed 
coordinate  system  determine  a fan-shaped  area.  The 
positions  of  all  objects  are  checked  against  this  fan-shaped 
area  to  identify  the  real  obstacles.  Only  those  objects 
which  are  inside  or  intersect  with  this  fan-shaped  area  are 
considered  to  be  obstacles. 

3.  Each  vertex  of  the  top  surfaces  of  the  obstacles 
and  the  Z-axis  determine  a vertical  plane.  These  planes  cut 
through  the  obstacles  and  wrist’s  workspace,  and  thus 
generate  the  cross  sections  of  the  obstacles  and  wrist's 
workspace.  These  cross  sections  are  then  transformed 
through  rotational  transformations  onto  the  X-Z  plane. 

4.  On  each  transformed  cross  section  of  the  wrist's 
workspace,  the  principal  obstacle  is  defined  as  the  obstacle 
first  touched  by  the  link  when  it  is  rotated  downwards  from 
its  highest  position.  There  are  three  cases  to  determine 
the  reachable  wrist  point. 

Case  1:  There  is  no  obstacle  higher  than  the  height  of 
joint  2 (see  figure  4.3). 


9 0 
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Figure  4.3  No  obstacle  is  higher  than 
the  height  of  joint  2 


Z 


Figure  4.4  The  obstacle  is  at  the  same 
height  as  joint  2 
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In  this  case,  the  angle  of  declination  for  an  obstacle 
is  defined  as  the  angle  between  the  horizontal  and  the  line 
passing  through  joint  2 and  the  exterior  top  corner  (the  top 
corner  furthest  from  the  origin)  of  the  obstacle.  The 
principal  obstacle  is  defined  as  the  obstacle  which  has  the 
smallest  angle  of  declination.  In  figure  4.3,  the  principal 
obstacle  is  A.  Line  segment  aS  is  the  lower  bound  of  the 
reachable  area  between  lines  X = X and  X = X„.  In 

Ci  S 

figure  4.4,  the  principal  obstacle  B is  at  the  same  height 
as  joint  2 and  the  angle  of  declination  is  zero. 

When  the  recommended  reachable  point  e is  located 
between  lines  X = X and  X = X„  and  lower  than  the  line 

ci  O 

segment  aS,  the  point  f on  the  line  segment  aS  and  with  the 
same  X coordinate  as  point  e is  selected  as  the  reachable 
wrist  point.  Obviously,  when  point  e is  higher  than  the 
line  segment  aS,  then  it  is  a real  reachable  point. 

Case  2:  The  highest  obstacle  is  higher  than  the  height 

of  joint  2 and  lower  than  the  upper  bound  of 
the  reachable  area  (see  figure  4.5). 

In  this  case,  the  angle  of  inclination  for  an  obstacle 
is  defined  as  the  angle  between  the  horizontal  and  the  line 
passing  through  joint  2 and  the  origin  side  top  corner  of 
the  obstacle.  The  principal  obstacle  is  defined  as  the 
obstacle  which  has  the  greatest  angle  of  inclination.  In 
figure  4.5,  the  principal  obstacle  is  B.  Point  S is  the 
extreme  position  of  the  wrist  when  the  link  touches  with  the 
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Figure  4.5  The  obstacle  B is  higher  than 

the  height  of  joint  2 and  lower 
than  the  upper  bound 


Z 


Figure  4.6  The  obstacle  B is  higher  than 
the  upper  bound 
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origin  side  top  corner  a of  the  principal  obstacle.  The 

line  segment  aS  is  the  lower  bound  of  the  reachable  area 

between  lines  X = X and  X = X_. 

a S 

When  the  X coordinate  of  the  recommended  reachable 
point  (for  example,  point  d)  is  greater  than  that  of  point 
S,  point  S is  selected  as  the  reachable  wrist  point.  When 
the  recommended  reachable  point  (for  example,  point  e)  is 
located  between  lines  X = X^  and  X = X^ , the  reachable 
wrist  point  (point  f)  is  determined  as  in  the  previous  case. 

Case  3:  There  is  one  obstacle  whose  origin  side  top 

corner  is  higher  than  the  upper  bound  of  the 
reachable  area  (see  figure  4.6) 

In  this  case,  the  reachable  wrist  point  (point  f)  can 
be  determined  by  the  corresponding  point  (point  e')  which  is 
on  the  origin  side  of  the  obstacle  and  has  the  same  Z 
coordinate  as  the  recommended  reachable  point  (point  e). 

5.  The  reachable  end  effector  points  are  determined 
from  the  corresponding  reachable  wrist  points  by  considering 
the  length  and  orientation  of  the  end  effector.  When  the 
end  effector  intersects  with  some  obstacle,  the  reachable 
end  effector  point  must  be  shifted  up  higher  than  this 
obstacle  to  avoid  collision. 

6.  After  the  reachable  end  effector  points  have  been 
found,  the  path  sequentially  connecting  all  those  reachable 
points  is  modified  to  be  a smooth  collision-free  path. 
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4 . 2 The  Cylindrical  Type  Robot 

The  second  joint  of  the  cylindrical  type  robot  is 
prismatic  and  this  is  the  essential  difference  with  the  polar 
type  robot  (see  figures  4.1  and  4.7).  This  prismatic  joint 
restricts  the  shoulder  and  arm  to  move  vertically  and 
horizontally.  The  procedures  for  determining  a 
collision-free  path  are  similar  to  those  developed  for  the 
polar  type  robot. 

1.  Here  the  expansion  of  objects  is  relatively  simple. 
Because  the  shoulder  and  arm  are  only  capable  of  vertical  and 
horizontal  sliding  motions,  the  reachable  sides  of  any  object 
are  simply  expanded  by  one  half  of  the  shoulder  width  or  arm 
width  depending  upon  which  touches  the  obstacle.  Figure  4.8 
shows  a top  view  picture  of  an  arbitrary  object  A shifted  out 
by  half  the  shoulder  width  together  with  the  projections  of 
the  cross  sections  of  the  shoulder  in  different  positions. 
Clearly,  those  cross  sections  cannot  touch  the  object, 

2.  The  method  of  expanding  an  object  for  the  polar  type 
robot  can  be  applied  to  this  type  robot  by  replacing  M and  N 
(see  figure  4.2),  which  have  been  defined  in  Chapter  II,  by 
half  the  widths  of  the  shoulder  and  arm  respectively.  The 
procedures  to  determine  an  Archimedes's  spiral  and  identify 
the  obstacles  are  also  the  same  as  those  applied  to  the  polar 
type  robot. 

3.  In  each  cross  section  of  the  wrist  reachable  space, 
the  principal  obstacle  is  defined  as  the  obstacle  lower  than 
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Figure  4.7  The  cylindrical  type  robot 


Figure  4.8  The  shape  of  obstacle  A 

betore  and  after  expansion 
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the  upper  bound  of  the  reachable  area  and  higher  than  other 
obstacles.  (see  obstacle  B of  figure  4.9).  The  method  to 
determinate  a reachable  wrist  point  is  similar  to  that  of 
the  polar  type  robot.  There  are  two  cases  which  must  be 
cons idered . 

Case  1;  There  is  no  obstacle  higher  than  the  upper 

bound  of  the  reachable  area  (see  figure  4.9). 

In  this  case,  the  lower  bound  of  the  reachable  area 

between  the  origin  side  of  the  principal  obstacle  B and  line 

X = Xg  is  the  horizontal  line  segment  aS.  When  the 

recommended  reachable  point  e is  lower  than  the  line  segment 

aS,  the  intersection  f of  line  X = X with  the  line 

e 

segment  aS  is  selected  as  the  reachable  wrist  point. 

Case  2:  There  is  an  obstacle  higher  than  the  upper 

bound  of  the  reachable  area  (see  figure  4.10). 

In  this  case,  no  reachable  point  above  the  recommended 
reachable  point  e can  be  found.  A reachable  point  f will  be 
determined  by  the  corresponding  point  e'  on  the  origin  side 
of  the  obstacle  and  at  the  same  height  as  the  recommended 
reachable  point  e. 

4.  After  all  the  reachable  wrist  points  have  been 
found,  the  corresponding  reachable  end  effector  points  can 
be  determined.  The  path  which  sequentially  connects  all 
reachable  end  effectors  points  is  then  modified  to  be  a 
smooth  collision-free  path  by  using  the  same 
developed  for  the  articulated  type  robot. 


method 
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Z 


Figure  4.9  No  obstacle  is  higher  than 
the  upper  bound  of  the 
reachable  area 


Z 


Figure  4.10  The  obstacle  B is  higher 
than  the  upper  bound  of 
the  reachable  area 
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4 . 3 The  Cartesian  Type  Robot 

The  first  joint  of  this  type  of  robot  is  prismatic  (see 
figure  4.11).  The  determination  of  a collision-free  path 
for  this  type  robot  is  similar  to  that  of  the  cylindrical 
type  robot.  This  is  because  the  first  revolute  joint  of  the 
cylindrical  robot  is  simply  replaced  by  a prismatic  joint. 

1.  The  wrist's  workspace  of  this  type  robot  is  a 
rectangular  solid.  Any  line  segment  connecting  a pair  of 
arbitrary  points  inside  the  rectangular  solid  also  lies 
inside  the  workspace.  Because  of  this,  a straight  line 
segment  instead  of  an  Archimedes's  spiral  from  the  initial 
to  goal  positions  is  chosen  as  a reference  path.  The 
projection  of  this  rectilinear  path  on  the  X-Y  plane  and  the 
horizontal  axis  of  the  first  joint  determine  a trapezoidal 
area.  This  trapezoid  like  the  fan-shaped  area  of  the 
articulated  type  robot  is  used  to  identify  the  real 
obstacles . 

2.  The  m.ethods  of  expanding  an  object  and  identifying 
the  obstacles  are  identical  to  those  of  the  cylindrical  type 
robot . 

3.  Each  of  the  top  vertices  of  the  obstacles 
determines  a vertical  plane  which  is  perpendicular  to  the 
first  joint  axis.  These  vertical  planes  determine  the  cross 
sections  of  the  wrist's  workspace  and  obstacles.  A 
reachable  wrist  point  in  each  cross  section  of  the  wrist's 
workspace  can  be  determined  using  the  method  developed  for 
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Figure  4.11  The  Cartesian  type  robot 
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the  cylindrical  type  robot.  After  all  reachable  wrist 

points  are  found,  the  corresponding  reachable  end  effector 

points  and  a collision-free  path  can  be  determined  by  using 

the  same  method  developed  for  the  articulated  type  robot. 

Using  the  above  procedures,  the  path  planning  algorithm 

developed  in  Chapters  II  and  III  can  be  applied  to  most 

industrial  robots.  An  application  of  this  algorithm  to  the 

3 

Cincinnati  Milacron  T -776  robot  in  an  obstacle  strewn 
environment  has  been  performed.  The  details  of  this 
application  are  discussed  in  the  next  chapter. 


CHAPTER  V 


COMPUTER  GRAPHICS  SIMULATION  OF  THE  ROBOT  MOTION 


Computer  graphics  constitutes  a significant 


contribution 

to 

the  ability 

to 

understand 

data 

and  to 

visualize  real 

or  imaginary 

objects . 

By 

making 

communication 

more  efficient. 

graphics  makes 

possible 

lower 

analysis  cost 

and 

also  design 

cost . 

In  this 

chapter , 

some 

aspects  of  the  generation  of  solid  modeling  pictures  and  the 
procedures  to  perform  motion  simulation  of  the  T^-776 
robot  are  discussed. 


5 . 1 Introduction  to  the  IRIS  2400  Workstation 
The  specifications  of  the  SILICON  GRAPHICS  IRIS  2400 
workstation  in  the  CIMAR  laboratory  are: 

CPU  Memory:  16  MHz  MC68010,  2MB  dynamic  RAM, 

expandable  to  16MB,  128KB  EPROM. 

Software:  UNIX  System  V operating  system. 

C Compiler. 

IRIS  Graphics  Library  II. 

Color  Range:  Color  map  mode(8-bit,  single  or  double 
buffered)  256  colors. 

Peripherals:  72  MB  5.25"  winchesteer  disk  drive. 

19"  diagonal  60  Hz,  1024x768  monitor. 
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Optical  mouse  X-Y  encoder  with  three 
buttons . 


5 . 2 Graphics  Techniques 

In  computer  graphics,  the  points  and  the  lines  which 
join  them  are  used  to  form  polygons.  These  polygons  are 
painted  to  display  solid  modeling  pictures  of  the  objects. 
Therefore,  an  object  in  the  three-dimensional  space  can  be 
relocated  to  any  position  by  transforming  the  points  on  it 
[38].  The  transformation  can  be  accomplished  by  using  the 
mathematical  technigues  of  translation,  rotation  and 
scaling.  The  translation  matrix  for  a point  [X,Y,Z,1]  in 
three-dimensional  space  is  defined  as  below. 


ri  0 0 o'" 


[T] 


0 10  0 
0 0 10 
Tx  Tv  Tz  1 


(5.1) 


where  Tx , Ty  and  Tz  are  the  translation  components  in 
the  X,  Y and  Z directions  respectively. 

The  rotation  matrix  about  an  arbitrary  axis  in 
three-dimensional  space  is  defined  by 


"r11  R12  R13  o'" 


[R] 


R21  R22  R23  0 
R31  R32  R33  0 


0 


0 


0 1 


(5.2) 
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where 


As  a 
Z-axis . 
case,  the 


Rll 

= 

Cx 

•k 

Cx 

+ 

(1 

- Cx  * 

Cx) 
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cos© 
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= 
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Cy 
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- cos©) 
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Cz 

* sin© 
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= 
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(1 
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Cx 

* sin© 

R31 

= 

Cx 
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Cz 
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(1 

- cos©) 

+ 

Cy 

* sin© 

R32 

= 

Cy 

k 

Cz 

k 

(1 

- cos©) 

+ 

Cx 

* sin© 

R33 

- 

Cz 

k 

Cz 

+ 

(1 

- Cz  * 

Cz) 

★ 

cos© 

9 = Rotation  angle  (radian) 

Cx , Cy  and  Cz  are  direction  cosines  of  the 
rotation  axis. 

specific  example  consider  rotation  about 
Then  Cx  = Cy  = 0 and  Cz  = 1.0.  For  this  spec 
rotation  matrix  [R]  reduces  to 


the 

ial 


[R] 


cos©  sin©  0 0 

-sin©  cos©  0 0 

0 0 10 

^ 0 0 0 1 


Rotation  is  assumed  to  be  positive  in  a right-hand 
sense  as  one  looks  from  the  origin  outwards  along  the  axis 
of  rotation.  The  procedure  of  rotation  about  any  axis  is  to 
translate  the  object  and  the  desired  axis  of  rotation  so 
that  the  rotation  is  made  about  an  axis  passing  through  the 
origin  of  the  fixed  coordinate  system.  The  method  involves 
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a three-dimensional  translation,  a rotation  about  the 
origin,  and  a translation  back  to  the  initial  position.  If. 
the  axis  about  which  rotation  is  desired  passes  through  the 
point  A = [Tx  Ty  Tz  1] , then  the  form  of  the  transformation 
matrix  is 


[Xn  Yn  Zn  1]  = [Xo  Yo  Zo  1]  [T]  [R]  [T] (5.3) 


where  Xn , Yn  and  Zn  are  the  transformed  coordinates. 
Xo,  Yo  and  Zo  are  the  original  coordinates. 
[T]^  is  given  by 


1 


0 


0 o' 


[T]’' 


0 10  0 

0 0 10 

-Tx  -Ty  -Tz  1 


After  transformation,  the  picture  can  also  be 
or  expanded  by  multiplying  a scaling  matrix.  The 
matrix  for  enlargement  or  reduction  is  defined  by 


reduced 

scaling 


[S] 


"a  0 0 o" 
0 b 0 0 
0 0 c 0 
^0  0 0 1^ 


(5.4) 


where  a,  b and  c are  the  multiples  of  enlargement  or 
reduction  in  the  X,  Y and  Z-directions 
respectively . 
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When  the  magnitudes  of  a,  b and  c are  unequal,  a 
distortion  occurs. 

The  above  graphic  technigues  are  often  used  to 
calculate  the  transformed  coordinates  of  points  which  will 
be  utilized  to  represent  or  display  three-dimensional 
objects.  In  figure  5.1,  object  R is  translated  by  the 
distance  in  the  Z-direction  from  the  origin.  Object  C 
is  translated  by  distances  Z^  and  -Y|^  along  the  Z and  Y 
directions  respectively  and  then  rotated  0 degrees  about  the 
negative  Y-axis. 

3 

5 . 3 Pictures  Generation  of  the  T -776  Robot 

3 

The  Cincinnati  Milacron  T -776  robot  is  an 
articulated  type  robot  with  six  revolute  joints.  When  given 
the  orientation  and  position  of  the  end  effector,  the 
angular  displacements  of  the  six  joints  can  be  calculated  by 
using  the  reverse  solution  developed  by  Duffy[39]. 
Generally,  there  may  be  eight  sets  of  joint  angles  which 
satisfy  the  given  conditions.  However,  there  is  only  one 
set  of  joint  angles  which  satisfies  the  joint  limitations 
and  which  has  the  least  incremental  change  compared  to  the 
set  of  joint  angles  for  the  previous  position.  This  set  is 
selected  to  reduce  the  response  time  of  the  actuators. 

3 

The  T -776  robot  can  be  divided  into  a variety  of 
components,  namely,  the  base,  prisms  la,  lb,  2a,  2b,  3,  4a, 
4b,  5,  6 and  the  wrist  (see  figure  5.2).  Each  component  is 
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Figure  5.1  The  relative  positions  among 
objects  A,  B and  C 
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The  components  of  the  T^-776 
robot 


Figure  5.2 
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assigned  a local  coordinate  system  to  define  the  coordinates 
of  the  points.  Provided  that  the  fixed  coordinate  system  is 
located  at  the  center  of  the  bottom  surface  of  the  base,  the 
positions  of  the  origins  of  all  local  coordinate  system 
corresponding  to  the  fixed  coordinate  system  can  be 
calculated  by  considering  simply  the  joint  angles  and  link 
lengths.  The  positions  of  the  origins  and  the  joint  angles 
found  above  are  used  to  transform  all  components  to  their 
appropriate  positions.  Furthermore,  each  of  the  component 
is  composed  of  several  polygons.  When  the  normal  vectors  of 
the  polygons  are  visible  from  an  specified  view  point,  the 
polygons  are  visible.  Otherwise,  they  are  not  visible  and 
it  is  not  necessary  to  draw  them.  In  addition,  a polygon 
may  be  covered  by  other  polygons  drawn  later.  Therefore, 
the  sequence  of  drawing  the  components  must  be  arranged 

3 

carefully.  A simple  and  fast  method  to  display  the  T -776 
robot  is  now  described. 

1.  Usually,  the  coordinate  system  of  a terminal  is  set 
at  the  left  bottom  corner  of  the  screen.  The  positive  X and 
Y-axes  are  the  abscissa  and  ordinate  respectively  and  the 
positive  Z-axis  points  out  of  the  screen.  The  three  axes 
determine  eight  quadrants  labeled  by  1,2,..., 8 which  are 
depended  on  the  signs  of  the  coordinates  (see  figure  5.3). 
The  space  composed  of  quadrants  1,  4,  5 and  8 is  called  the 
front  half  space;  while  the  space  composed  of  quadrants  1 
and  4 is  called  the  upper  half  space.  A surface  of  any 
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Figure  5.3  The  signs  of  the  coordinates 
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Figure  5.4  The  flow  chart  of  drawing  the  T -776  robot 
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component  of  the  robot  is  visible  only  when  its  normal 
vector  belongs  to  the  front  half  space. 

2.  The  sequence  of  displaying  the  components  can  be 
arranged  based  on  the  orientations  of  the  positive  Z,  Y'  and 
X"-axes  (see  figure  5.4).  The  components  are  drawn 
sequentially  one  after  another. 

5 . 4 Motion  Simulation  of  the  T -776  Robot 

An  example  of  the  application  of  the  path  generation 
algorithm  developed  in  the  previous  chapters  is  displayed  in 
figure  1.1.  The  heights  of  the  initial  and  goal  positions 
and  the  pre-determined  Archimedes's  spiral  path  is  shown  in 
figure  2.16.  The  objects  D,  E and  F are  outside  the 
fan-shaped  area.  As  described  before,  they  are  removed 
without  further  consideration;  while  objects  A,  B and  C are 
expanded  to  enable  the  shoulder  and  arm  to  be  considered  as 
line  segments.  The  expanded  objects  together  with  the 
initial  Archimedes’s  spiral  are  illustrated  in  figure  5.5. 

Figure  5.6  shows  all  vertical  planes  determined  by  the 
Z-axis  and  the  vertices  of  the  top  surfaces  of  obstacles  A, 
B and  C.  Each  of  these  planes  generates  the  cross  sections 
of  the  wrist's  workspace  and  obstacles.  Figure  5.7 
illustrates  a projection  of  one  of  the  cross  sections  of  the 
wrist’s  workspace  and  obstacles  on  the  X-Z  plane.  Further 
the  labels  "+"  and  "4"  are  used  to  indicate  the  projections 
of  the  initial  and  goal  positions  in  the  X-Z  plane  while  "o" 


Ill 


Figure  5.5  The  robot  and  expanded  obstacles 


Figure  5.6  The  vertical  planes  cut  through  the 
obstacles 
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Figure  5.7  The  end  effector  moves  straight 
towards  the  goal  position 


Figure  5.8  The  end  effector  moves  upwards  to 
avoid  collision 
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represents  the  projection  of  the  reachable  end  effector 
point  in  the  previous  cross  section.  It  should  be  noted 
that  when  the  manipulator  is  not  prevented  from  moving 
towards  the  goal  position  by  an  obstacle,  the  end  effector 
is  driven  along  the  line  segment  joining  "+"  and  "A". 
However,  when  the  manipulator  is  prevented  from  moving 
towards  the  goal  position  by  an  obstacle,  it  is  raised 
higher  to  avoid  collision.  For  instance  (see  figure  5.8), 
instead  of  moving  from  "a"  to  "A"  on  a straight  line,  the 
end  effector  point  is  raised  parallel  to  the  Z-axis  to  avoid 
an  obstacle.  In  figure  5.9,  the  end  effector  is  moved  from 
the  reachable  point  "o"  in  the  previous  cross  section  to  the 
new  position  above  the  lower  bound  of  the  reachable  area  to 
avoid  collision  (see  section  3.2  for  details).  After  the 
manipulator  has  been  guided  over  the  obstacle  in  this  way, 
it  moves  straight  again  towards  the  goal  position  (see 
figure  5.10) 

The  reachable  points  found  in  all  cross  sections  are 
modified  and  then  connected  seguentially  to  produce  a smooth 
collision-free  path.  The  sequential  motion  of  the 
manipulator  along  the  modified  path  is  illustrated  by  the 
series  of  figures  5.11  to  5.17.  The  program  has  been 
excuted  on  a VAX  11/750  computer.  The  total  CPU  time  to 
find  the  modified  path  is  less  than  two  seconds. 
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Figure  5.9  The  end  effector  moves  over  the 
obstacle 


Figure  5.10  The  end  effector  moves  towards 
the  goal  position  again 
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Figure  5.11  Motion  simulation  - 1 


Figure  5.12  Motion  simulation  - 2 
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Figure  5.14  Motion  simulation  - 4 
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Figure  5.16  Motion  simulation  - 6 
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Figure  5.17  Motion  simulation  - 7 


CHAPTER  VI 


CONCLUSIONS  AND  RECOMMENDATIONS 

In  this  dissertation,  an  algorithm  to  automatically 
generate  a collision-free  path  for  robot  manipulators  has 
been  developed.  This  algorithm  can  increase  the  autonomous 
ability  of  a robot  manipulator  operating  in  a 
obstacle-strewn  environment.  The  algorithm  has  been 
implemented  and  validated  using  the  computer  graphics 
pictures  display  of  the  seguential  motion  of  the  robot 
manipulator . 

It  is  anticipated  that  a modification  of  the  algorithm 
developed  in  this  dissertation  could  be  used  when  the  end 
effector  grasps  an  object.  In  such  a case,  the  collision 
avoidance  of  the  end  effector  and  grasped  object  with 
surrounding  obstacles  must  be  considered  together  with  the 
determination  of  the  reachable  wrist  points. 

Another  suggested  extension  is  to  consider  movable 
obstacles.  In  this  case,  the  environment  is  changed  at  any 
instance.  Theoretically,  it  is  necessary  to  continuously 
check  whether  there  is  collision  between  the  manipulator  and 
obstacles.  However,  when  the  obstacles  are  far  away  from 
the  manipulator,  the  algorithm  should  have  the  abililty  to 
check  in  what  time  period  the  motion  is  free  from  collision. 
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When  a pair  of  robot  manipulators  work  together,  the 
links  of  one  of  the  robots  are  considered  as  movable 
objects.  The  algorithm  developed  for  above  case  can  be 
modified  to  apply  to  this  case. 

Finally,  the  algorithm  needs  to  be  extended  to  include 
obstacles  with  inclided  faces.  The  determination  of  the 
lower  bound  of  the  wrist's  reachable  area  is  more 
complicated  for  such  obstacles. 

Following  the  development  of  computer  software,  the 
technigues  of  artificial  intelligence  and  expert  system  are 
useful  in  solving  decision-making  problems.  It  is  a wise 
choice  to  solve  path  planning  problems  by  applying  computer 
techniques  and  geometric  theory. 


APPENDIX  A 


where 


Three 
relat i 


Z 


h:  the  offset  of  joint  1. 

a:  the  length  of  link  2. 

b:  the  length  of  link  3. 

x:  the  X coordinate  of  the  wrist. 

1:  the  height  of  the  obstacle, 
eguations  can  be  derived  through  the  geometric 
on  of  the  above  figure. 

tantf)  = (c  - z)  / (x  - d)  (1) 

asin©  = bsincj)  + z (2) 

acos©  = X - bcoscj)  (3) 
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( 2 ) ^ + ( 3 ) ^ yields 


?■  = (bsiri(j5  + z)^  + (x  - bcos(|)) 


(4) 


Solving  equation  (4)  yields 


zsincf)  - xcos(|>  = 


a2  - b2  - - x2 


2b 


(5) 


From  (1),  sincj)  and  coscj)  are  calculated  and  given  by 

(6) 
(7) 


sincj)  = (c-z)  / ((x-d)^  + (c-z)^)^/^ 


coscj)  = (x-d)  / ((x-d)2  + (c-z)^)^/^ 


Substituting  (6)  and  (7)  into  (5)  yields 


z(c-z)  - x(x-d) 

( (x-d)^  + (c-z)^)^/^ 


a2  - b2  - z2  - x2 


2b 


(8) 


which  can  be  expressed  in  the  form 


(x-d)^  + (c-z)^  = 


(2b(z(c-z)  - x(x-d))) 


(a^  - b^  - z^  - x^)^  (9) 


or 


+ (c-z)^ 


( 2b( z( c-z)  - xp) ) 


, 2.2 

(q  - z ) 


(10) 


where  p = x - d 


2 k2  ^2 

q = a - b - X 
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Expanding  (10)  yields 


+ 

slz^  + s2z^  + s3z^  + s4z^  + s5z  + 

where  si 

= 

-2c 

s2 

= 

2 2 2 
- 2q  - 4b^ 

s3 

= 

4qc  + 8cb^ 

s4 

= 

q(q  - 2(c^  + p^))  - 4b^(c^  + 2xp) 

s5 

= 

-2q^c  + Sb^xpc 

s6 

= 

2 2 2 2 
q (c  + p^)  - (2bxp)^ 

s6  = 0 
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